#include "config.h"
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <string.h>         // strncpy()
#include <ctype.h>          // isspace()
#include "nml_intf/zuc.hh"  // ZUC NML
#include "nml_intf/zuc_nml.hh"
#include "nml_intf/canon.hh"
#include "nml_intf/canon_position.hh"  // data type for a machine position
#include "nml_intf/interpl.hh"         // interp_list
#include "nml_intf/zucglb.h"           // TRAJ_MAX_VELOCITY
#include "rcs_print.hh"
#include "motion/usrmotintf.h"
#include <pthread.h>
#include "kinematics/kinematics.h"
#include "task/motion_al.h"

/*kinematics flags maintained for ngc interpreter*/
unsigned long interp_fflags = 0;
unsigned long interp_iflags = 0;

// extern int kinematicsForward(const ZucPose* toolOffset,
//                              const double* joint,
//                              ZucPose* world,
//                              const unsigned long* interp_fflags,
//                              unsigned long* interp_iflags,
//                              const PmRpy* base_offset,
//                              const ZucPose* user_offset);
// extern int kinematicsInverse(const ZucPose* toolOffset,
//                              const ZucPose* world,
//                              double* joint,
//                              const unsigned long* interp_iflags,
//                              unsigned long* interp_fflags,
//                              const PmRpy* base_offset,
//                              const ZucPose* user_offset);
extern int getMovcEnd(ZucPose const* const start, ZucPose const* const mid, ZucPose const* const end, double count, ZucPose* out);
extern int getToolMovPos(const ZucPose* world, const ZucPose* pos, ZucPose* out);

//#define ZUCCANON_DEBUG

//Simple compile-time debug macro
#ifdef ZUCCANON_DEBUG
#define canon_debug(...) printf(__VA_ARGS__)
#else
#define canon_debug(...)
#endif

/*
  Origin offsets, length units, and active plane are all maintained
  here in this file. Controller runs in absolute mode, and does not
  have plane select concept.

  programOrigin is stored in mm always, and converted when set or read.
  When it's applied to positions, convert positions to mm units first
  and then add programOrigin.

  Units are then converted from mm to external units, as reported by
  the GET_EXTERNAL_LENGTH_UNITS() function.
  */

static CanonConfig_t canon;

static int debug_velacc = 0;
static const double tiny = 1e-7;

#ifndef MIN
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#endif

#ifndef MIN3
#define MIN3(a, b, c) (MIN(MIN((a), (b)), (c)))
#endif

#ifndef MAX
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#endif

#ifndef MAX3
#define MAX3(a, b, c) (MAX(MAX((a), (b)), (c)))
#endif

#ifndef MAX4
#define MAX4(a, b, c, d) (MAX(MAX((a), (b)), MAX((c), (d))))
#endif

#ifndef MAX9
#define MAX9(a, b, c, d, e, f, g, h, i) (MAX3((MAX3(a, b, c)), (MAX3(d, e, f)), (MAX3(g, h, i))))
#endif

/* macros for converting internal (mm/deg) units to external units */
#define TO_EXT_LEN(mm) ((mm)*GET_EXTERNAL_LENGTH_UNITS())
#define TO_EXT_ANG(deg) ((deg)*GET_EXTERNAL_ANGLE_UNITS())

/* macros for converting external units to internal (mm/deg) units */
#define FROM_EXT_LEN(ext) ((ext) / GET_EXTERNAL_LENGTH_UNITS())
#define FROM_EXT_ANG(ext) ((ext) / GET_EXTERNAL_ANGLE_UNITS())

/* macros for converting internal (mm/deg) units to program units */
#define TO_PROG_LEN(mm) ((mm) / (canon.lengthUnits == CANON_UNITS_INCHES ? 25.4 : canon.lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define TO_PROG_ANG(deg) (deg)

/* macros for converting program units to internal (mm/deg) units */
#define FROM_PROG_LEN(prog) ((prog) * (canon.lengthUnits == CANON_UNITS_INCHES ? 25.4 : canon.lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define FROM_PROG_ANG(prog) (prog)

/* Certain axes are periodic.  Hardcode this for now */
#define IS_PERIODIC(axisnum) ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5)

// this doesn't quite work yet: disable
#undef IS_PERIODIC
#define IS_PERIODIC(axisnum) (0)

#define AXIS_PERIOD(axisnum) (IS_PERIODIC(axisnum) ? 360 : 0)

//KLUDGE kinematic data struct (instead of returning a single float value)
//FIXME This should really be refactored into a more general structure, but this
//means tearing up the getStraightXXX functions, which probably means
//converting to canon_position operators
struct VelData
{
    double tmax;
    double vel;
    double dtot;
    UsingAxis axis;
};

struct AccelData
{
    double tmax;
    double acc;
    double dtot;
};

static PM_QUATERNION quat(1, 0, 0, 0);

static void flush_segments(void);

/*
  These decls were from the old 3-axis canon.hh, and refer functions
  defined here that are used for convenience but no longer have decls
  in the 6-axis canon.hh. So, we declare them here now.
*/
extern void CANON_ERROR(const char* fmt, ...) __attribute__((format(printf, 1, 2)));

#ifndef D2R
#define D2R(r) ((r)*M_PI / 180.0)
#endif

// static void rotate(double &x, double &y, double theta) {
//     double xx, yy;
//     double t = D2R(theta);
//     xx = x, yy = y;
//     x = xx * cos(t) - yy * sin(t);
//     y = xx * sin(t) + yy * cos(t);
// }

// static void to_rotated(PM_CARTESIAN &vec) {
//     rotate(vec.x,vec.y,0);
// }

static void rotate_and_offset(CANON_POSITION& pos) { pos += canon.toolOffset; }

static void rotate_and_offset_xyz(PM_CARTESIAN& xyz) { xyz += PM_CARTESIAN(canon.toolOffset.tran.x, canon.toolOffset.tran.y, canon.toolOffset.tran.z); }

static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION pos)
{
    CANON_POSITION res;

    res = pos;

    res -= canon.toolOffset;

    return res;
}

static void rotate_and_offset_pos(double& x, double& y, double& z, double& a, double& b, double& c, double& u, double& v, double& w)
{
    x += canon.toolOffset.tran.x;
    y += canon.toolOffset.tran.y;
    z += canon.toolOffset.tran.z;
    a += canon.toolOffset.a;
    b += canon.toolOffset.b;
    c += canon.toolOffset.c;
    u += canon.toolOffset.u;
    v += canon.toolOffset.v;
    w += canon.toolOffset.w;
}

// static CANON_POSITION unoffset_and_unrotate_pos(const ZucPose pos) {
//     CANON_POSITION res(pos);
//     return unoffset_and_unrotate_pos(res);
// }

static void from_prog(double& x, double& y, double& z, double& a, double& b, double& c, double& u, double& v, double& w)
{
    x = FROM_PROG_LEN(x);
    y = FROM_PROG_LEN(y);
    z = FROM_PROG_LEN(z);
    a = FROM_PROG_ANG(a);
    b = FROM_PROG_ANG(b);
    c = FROM_PROG_ANG(c);
    u = FROM_PROG_LEN(u);
    v = FROM_PROG_LEN(v);
    w = FROM_PROG_LEN(w);
}

static void from_prog(CANON_POSITION& pos)
{
    pos.x = FROM_PROG_LEN(pos.x);
    pos.y = FROM_PROG_LEN(pos.y);
    pos.z = FROM_PROG_LEN(pos.z);
    pos.a = FROM_PROG_ANG(pos.a);
    pos.b = FROM_PROG_ANG(pos.b);
    pos.c = FROM_PROG_ANG(pos.c);
    pos.u = FROM_PROG_LEN(pos.u);
    pos.v = FROM_PROG_LEN(pos.v);
    pos.w = FROM_PROG_LEN(pos.w);
}

static void from_prog_len(PM_CARTESIAN& vec)
{
    vec.x = FROM_PROG_LEN(vec.x);
    vec.y = FROM_PROG_LEN(vec.y);
    vec.z = FROM_PROG_LEN(vec.z);
}
#if 0
static void to_ext(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) {
    x = TO_EXT_LEN(x);
    y = TO_EXT_LEN(y);
    z = TO_EXT_LEN(z);
    a = TO_EXT_ANG(a);
    b = TO_EXT_ANG(b);
    c = TO_EXT_ANG(c);
    u = TO_EXT_LEN(u);
    v = TO_EXT_LEN(v);
    w = TO_EXT_LEN(w);
}

static void to_ext(CANON_POSITION & pos) {
    pos.x=TO_EXT_LEN(pos.x);
    pos.y=TO_EXT_LEN(pos.y);
    pos.z=TO_EXT_LEN(pos.z);
    pos.a=TO_EXT_ANG(pos.a);
    pos.b=TO_EXT_ANG(pos.b);
    pos.c=TO_EXT_ANG(pos.c);
    pos.u=TO_EXT_LEN(pos.u);
    pos.v=TO_EXT_LEN(pos.v);
    pos.w=TO_EXT_LEN(pos.w);
}
#endif

static PM_CARTESIAN to_ext_len(const PM_CARTESIAN& pos)
{
    PM_CARTESIAN ret;
    ret.x = TO_EXT_LEN(pos.x);
    ret.y = TO_EXT_LEN(pos.y);
    ret.z = TO_EXT_LEN(pos.z);
    return ret;
}

static ZucPose to_ext_pose(double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    ZucPose result;
    result.tran.x = TO_EXT_LEN(x);
    result.tran.y = TO_EXT_LEN(y);
    result.tran.z = TO_EXT_LEN(z);
    result.a = TO_EXT_ANG(a);
    result.b = TO_EXT_ANG(b);
    result.c = TO_EXT_ANG(c);
    result.u = TO_EXT_LEN(u);
    result.v = TO_EXT_LEN(v);
    result.w = TO_EXT_LEN(w);
    return result;
}

static ZucPose to_ext_pose(const CANON_POSITION& pos)
{
    ZucPose result;
    result.tran.x = TO_EXT_LEN(pos.x);
    result.tran.y = TO_EXT_LEN(pos.y);
    result.tran.z = TO_EXT_LEN(pos.z);
    result.a = TO_EXT_ANG(pos.a);
    result.b = TO_EXT_ANG(pos.b);
    result.c = TO_EXT_ANG(pos.c);
    result.u = TO_EXT_LEN(pos.u);
    result.v = TO_EXT_LEN(pos.v);
    result.w = TO_EXT_LEN(pos.w);
    return result;
}

static void to_prog(CANON_POSITION& e)
{
    e.x = TO_PROG_LEN(e.x);
    e.y = TO_PROG_LEN(e.y);
    e.z = TO_PROG_LEN(e.z);
    e.a = TO_PROG_ANG(e.a);
    e.b = TO_PROG_ANG(e.b);
    e.c = TO_PROG_ANG(e.c);
    e.u = TO_PROG_LEN(e.u);
    e.v = TO_PROG_LEN(e.v);
    e.w = TO_PROG_LEN(e.w);
}

static int axis_valid(int n) { return zucStatus->motion.traj.axis_mask & (1 << n); }

static void canonUpdateEndPoint(double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    canon.endPoint.x = x;
    canon.endPoint.y = y;
    canon.endPoint.z = z;

    canon.endPoint.a = a;
    canon.endPoint.b = b;
    canon.endPoint.c = c;

    canon.endPoint.u = u;
    canon.endPoint.v = v;
    canon.endPoint.w = w;
}

static void canonUpdateEndPoint(const CANON_POSITION& pos) { canon.endPoint = pos; }

/* External call to update the canon end point.
   Called by zuctask during skipping of lines (run-from-line) */
void CANON_UPDATE_END_POINT(double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    canonUpdateEndPoint(FROM_PROG_LEN(x),
                        FROM_PROG_LEN(y),
                        FROM_PROG_LEN(z),
                        FROM_PROG_ANG(a),
                        FROM_PROG_ANG(b),
                        FROM_PROG_ANG(c),
                        FROM_PROG_LEN(u),
                        FROM_PROG_LEN(v),
                        FROM_PROG_LEN(w));
}

static double toExtVel(double vel)
{
    if (canon.cartesian_move && !canon.angular_move)
    {
        return TO_EXT_LEN(vel);
    }
    else if (!canon.cartesian_move && canon.angular_move)
    {
        return TO_EXT_ANG(vel);
    }
    else if (canon.cartesian_move && canon.angular_move)
    {
        return TO_EXT_LEN(vel);
    }
    else
    {  //seems this case was forgotten, neither linear, neither angular move (we are only sending vel)
        return TO_EXT_LEN(vel);
    }
}

static double toExtAcc(double acc) { return toExtVel(acc); }

void USE_LENGTH_UNITS(CANON_UNITS in_unit)
{
    canon.lengthUnits = in_unit;

    zucStatus->task.programUnits = in_unit;
}

/* Free Space Motion */
void SET_TRAVERSE_RATE(double rate)
{
    // nothing need be done here
}

void SET_FEED_MODE(int mode)
{
    flush_segments();
    canon.feed_mode = mode;
    if (canon.feed_mode == 0)
        STOP_SPEED_FEED_SYNCH();
}

void SET_FEED_RATE(double rate)
{
    if (canon.feed_mode)
    {
        START_SPEED_FEED_SYNCH(rate, 1);
        canon.linearFeedRate = rate;
    }
    else
    {
        /* convert from /min to /sec */
        rate /= 60.0;

        /* convert to traj units (mm & deg) if needed */
        double newLinearFeedRate = FROM_PROG_LEN(rate), newAngularFeedRate = FROM_PROG_ANG(rate);

        if (newLinearFeedRate != canon.linearFeedRate || newAngularFeedRate != canon.angularFeedRate)
            flush_segments();

        canon.linearFeedRate = newLinearFeedRate;
        canon.angularFeedRate = newAngularFeedRate;
    }
}

void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference)
{
    // nothing need be done here
}

/**
 * Get the limiting acceleration for a displacement from the current position to the given position.
 * returns a single acceleration that is the minimum of all axis accelerations.
 */
static AccelData getStraightAcceleration(double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    double dx, dy, dz, du, dv, dw, da, db, dc;
    double tx, ty, tz, tu, tv, tw, ta, tb, tc;
    AccelData out;

    out.acc = 0.0;  // if a move to nowhere
    out.tmax = 0.0;
    out.dtot = 0.0;

    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return out;
    }

    // Compute absolute travel distance for each axis:
    dx = fabs(x - canon.endPoint.x);
    dy = fabs(y - canon.endPoint.y);
    dz = fabs(z - canon.endPoint.z);
    da = fabs(a - canon.endPoint.a);
    db = fabs(b - canon.endPoint.b);
    dc = fabs(c - canon.endPoint.c);
    du = fabs(u - canon.endPoint.u);
    dv = fabs(v - canon.endPoint.v);
    dw = fabs(w - canon.endPoint.w);

    if (!axis_valid(0) || dx < tiny)
        dx = 0.0;
    if (!axis_valid(1) || dy < tiny)
        dy = 0.0;
    if (!axis_valid(2) || dz < tiny)
        dz = 0.0;
    if (!axis_valid(3) || da < tiny)
        da = 0.0;
    if (!axis_valid(4) || db < tiny)
        db = 0.0;
    if (!axis_valid(5) || dc < tiny)
        dc = 0.0;
    if (!axis_valid(6) || du < tiny)
        du = 0.0;
    if (!axis_valid(7) || dv < tiny)
        dv = 0.0;
    if (!axis_valid(8) || dw < tiny)
        dw = 0.0;

    if (debug_velacc)
        printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", dx, dy, dz, da, db, dc, du, dv, dw);

    // Figure out what kind of move we're making.  This is used to determine
    // the units of vel/acc.
    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && du <= 0.0 && dv <= 0.0 && dw <= 0.0)
    {
        canon.cartesian_move = 0;
    }
    else
    {
        canon.cartesian_move = 1;
    }
    if (da <= 0.0 && db <= 0.0 && dc <= 0.0)
    {
        canon.angular_move = 0;
    }
    else
    {
        canon.angular_move = 1;
    }

    // Pure linear move:
    if (canon.cartesian_move && !canon.angular_move)
    {
        tx = dx ? (dx / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(0))) : 0.0;
        ty = dy ? (dy / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(1))) : 0.0;
        tz = dz ? (dz / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(2))) : 0.0;
        tu = du ? (du / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(6))) : 0.0;
        tv = dv ? (dv / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(7))) : 0.0;
        tw = dw ? (dw / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(8))) : 0.0;
        out.tmax = MAX3(tx, ty, tz);
        out.tmax = MAX4(tu, tv, tw, out.tmax);

        if (dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax > 0.0)
        {
            out.acc = out.dtot / out.tmax;
            if (out.acc > FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxAcceleration()))
            {
                out.acc = FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxAcceleration());
            }
        }
    }
    // Pure angular move:
    else if (!canon.cartesian_move && canon.angular_move)
    {
        ta = da ? (da / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3))) : 0.0;
        tb = db ? (db / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(4))) : 0.0;
        tc = dc ? (dc / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(5))) : 0.0;
        out.tmax = MAX3(ta, tb, tc);

        out.dtot = sqrt(da * da + db * db + dc * dc);
        if (out.tmax > 0.0)
        {
            out.acc = out.dtot / out.tmax;
        }
    }
    // Combination angular and linear move:
    else if (canon.cartesian_move && canon.angular_move)
    {
        tx = dx ? (dx / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(0))) : 0.0;
        ty = dy ? (dy / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(1))) : 0.0;
        tz = dz ? (dz / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(2))) : 0.0;
        ta = da ? (da / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3))) : 0.0;
        tb = db ? (db / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(4))) : 0.0;
        tc = dc ? (dc / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(5))) : 0.0;
        tu = du ? (du / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(6))) : 0.0;
        tv = dv ? (dv / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(7))) : 0.0;
        tw = dw ? (dw / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(8))) : 0.0;
        out.tmax = MAX9(tx, ty, tz, ta, tb, tc, tu, tv, tw);

        if (debug_velacc)
            printf("getStraightAcceleration t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", tx, ty, tz, ta, tb, tc, tu, tv, tw);
        /*  According to NIST IR6556 Section 2.1.2.5 Paragraph A
    a combnation move is handled like a linear move, except
    that the angular axes are allowed sufficient time to
    complete their motion coordinated with the motion of
    the linear axes.
*/
        if (dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax > 0.0)
        {
            out.acc = out.dtot / out.tmax;
            if (out.acc > FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxAcceleration()))
            {
                out.acc = FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxAcceleration());
            }
        }
    }
    if (debug_velacc)
        printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc);
    return out;
}

static AccelData getStraightAcceleration(CANON_POSITION pos) { return getStraightAcceleration(pos.x, pos.y, pos.z, pos.a, pos.b, pos.c, pos.u, pos.v, pos.w); }

static VelData getStraightVelocity(double x, double y, double z, double a, double b, double c, double u, double v, double w, double vel)
{
    double dx, dy, dz, da, db, dc, du, dv, dw;
    double tx, ty, tz, ta, tb, tc, tu, tv, tw;
    VelData out;
    //UsingAxis axis;

    out.axis = USING_X;
    out.vel = canon.linearFeedRate;
    out.tmax = 0;
    out.dtot = 0;

    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return out;
    }

    // rcs_print("[getStraightVelocity] : current endPoint: %f, %f, %f, %f, %f, %f\n", canon.endPoint.x, canon.endPoint.y, canon.endPoint.z, canon.endPoint.a, canon.endPoint.b, canon.endPoint.c);
    // rcs_print("[getStraightVelocity] : endPoint: %f, %f, %f, %f, %f, %f\n", x,y,z,a,b,c);
    // Compute absolute travel distance for each axis:
    dx = fabs(x - canon.endPoint.x);
    dy = fabs(y - canon.endPoint.y);
    dz = fabs(z - canon.endPoint.z);
    da = fabs(a - canon.endPoint.a);
    if (da > 180.0)
    {
        da = fabs(da - 360.0);
    }
    db = fabs(b - canon.endPoint.b);
    if (db > 180.0)
    {
        db = fabs(db - 360.0);
    }
    dc = fabs(c - canon.endPoint.c);
    if (dc > 180.0)
    {
        dc = fabs(dc - 360.0);
    }
    du = fabs(u - canon.endPoint.u);
    dv = fabs(v - canon.endPoint.v);
    dw = fabs(w - canon.endPoint.w);

    if (!axis_valid(0) || dx < tiny)
        dx = 0.0;
    if (!axis_valid(1) || dy < tiny)
        dy = 0.0;
    if (!axis_valid(2) || dz < tiny)
        dz = 0.0;
    if (!axis_valid(3) || da < tiny)
        da = 0.0;
    if (!axis_valid(4) || db < tiny)
        db = 0.0;
    if (!axis_valid(5) || dc < tiny)
        dc = 0.0;
    if (!axis_valid(6) || du < tiny)
        du = 0.0;
    if (!axis_valid(7) || dv < tiny)
        dv = 0.0;
    if (!axis_valid(8) || dw < tiny)
        dw = 0.0;

    // rcs_print("\n\ngetStraightVelocity dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g\n\n",
    //            dx, dy, dz, da, db, dc, du, dv, dw);

    // Figure out what kind of move we're making:
    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && du <= 0.0 && dv <= 0.0 && dw <= 0.0)
    {
        canon.cartesian_move = 0;
    }
    else
    {
        canon.cartesian_move = 1;
    }
    if (da <= 0.0 && db <= 0.0 && dc <= 0.0)
    {
        canon.angular_move = 0;
    }
    else
    {
        canon.angular_move = 1;
    }

    // Pure linear move:
    if (canon.cartesian_move && !canon.angular_move)
    {
        tx = dx ? fabs(dx / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(0))) : 0.0;
        ty = dy ? fabs(dy / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(1))) : 0.0;
        tz = dz ? fabs(dz / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(2))) : 0.0;
        tu = du ? fabs(du / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(6))) : 0.0;
        tv = dv ? fabs(dv / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(7))) : 0.0;
        tw = dw ? fabs(dw / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(8))) : 0.0;
        out.tmax = MAX3(tx, ty, tz);
        out.tmax = MAX4(tu, tv, tw, out.tmax);

        if (dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax <= 0.0)
        {
            out.vel = canon.linearFeedRate;
        }
        else
        {
            out.vel = out.dtot / out.tmax;
            if (out.vel > FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel()))
            {
                out.vel = FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel());
            }
        }
    }
    // Pure angular move:
    else if (!canon.cartesian_move && canon.angular_move)
    {
        ta = da ? fabs(da / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3))) : 0.0;
        tb = db ? fabs(db / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(4))) : 0.0;
        tc = dc ? fabs(dc / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(5))) : 0.0;
        out.tmax = MAX3(ta, tb, tc);
        out.axis = USING_A;
        out.dtot = sqrt(da * da + db * db + dc * dc);
        if (out.tmax <= 0.0)
        {
            out.vel = canon.angularFeedRate;
        }
        else
        {
            out.vel = out.dtot / out.tmax;
        }
    }
    // Combination angular and linear move:
    else if (canon.cartesian_move && canon.angular_move)
    {
        out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        out.tmax = out.dtot / fabs(vel);
        // rcs_print("new: out.tmax=%f, out.dtot=%f, fabs(vel)=%f\n", out.tmax, out.dtot, fabs(vel));

        // rpy to qurternion
        PmRpy rpyTarget, rpyCurr;
        PmQuaternion quatTarget, quatCurr, quatDelt;
        rpyTarget.y = a * PM_PI / 180.0;
        rpyTarget.p = b * PM_PI / 180.0;
        rpyTarget.r = c * PM_PI / 180.0;
        rpyCurr.y = canon.endPoint.a * PM_PI / 180.0;
        rpyCurr.p = canon.endPoint.b * PM_PI / 180.0;
        rpyCurr.r = canon.endPoint.c * PM_PI / 180.0;
        pmRpyQuatConvert(&rpyTarget, &quatTarget);
        pmRpyQuatConvert(&rpyCurr, &quatCurr);
        pmQuatInv(&quatCurr, &quatCurr);
        pmQuatQuatMult(&quatTarget, &quatCurr, &quatDelt);
        double deltAngle = fabs(2 * acos(quatDelt.s) * 180.0 / PM_PI);
        ta = deltAngle ? fabs(deltAngle / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3))) : 0.0;
        // rcs_print("new: quatDelt.s = %f, deltAngle = %f, ta = %f\n", quatDelt.s, deltAngle, ta);
        if (out.tmax < ta)
        {
            out.tmax = ta;
            out.axis = USING_A;
            out.dtot = deltAngle;
        }
        else
        {
            out.axis = USING_X;
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        }

        // ta = da? fabs(da / FROM_EXT_ANG(zucAxisGetMaxVelocity(3))): 0.0;
        // tb = db? fabs(db / FROM_EXT_ANG(zucAxisGetMaxVelocity(4))): 0.0;
        // tc = dc? fabs(dc / FROM_EXT_ANG(zucAxisGetMaxVelocity(5))): 0.0;

        // if(out.tmax < ta)
        // {
        //     out.tmax = ta;
        //     out.axis = USING_A;
        // }
        // if(out.tmax < tb)
        // {
        //     out.tmax = tb;
        //     out.axis = USING_B;
        // }
        // if(out.tmax < tc)
        // {
        //     out.tmax = tc;
        //     out.axis = USING_C;
        // }

        // if(out.axis > USING_X)
        // {
        //     out.dtot = sqrt(da * da + db * db + dc * dc);
        // }
        // else
        // {
        //     out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        // }

        if (out.tmax <= 0.0)
        {
            out.vel = canon.linearFeedRate;
        }
        else
        {
            out.vel = out.dtot / out.tmax;
            if (USING_X == out.axis)
            {
                if (out.vel > FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel()))
                {
                    out.vel = FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel());
                }
            }
        }
    }
    if (debug_velacc)
        printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel);
    return out;
}

static VelData
getStraightVelocity_movl(double x, double y, double z, double a, double b, double c, double u, double v, double w, double vel, double acc, int* alread_inpos)
{
    double dx, dy, dz, da, db, dc, du, dv, dw;
    double tx, ty, tz, ta, tb, tc, tu, tv, tw;
    VelData out;
    //UsingAxis axis;

    out.axis = USING_X;
    out.vel = canon.linearFeedRate;
    out.tmax = 0;
    out.dtot = 0;

    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return out;
    }

    // rcs_print("[getStraightVelocity] : current endPoint: %f, %f, %f, %f, %f, %f\n", canon.endPoint.x, canon.endPoint.y, canon.endPoint.z, canon.endPoint.a, canon.endPoint.b, canon.endPoint.c);
    // rcs_print("[getStraightVelocity] : endPoint: %f, %f, %f, %f, %f, %f\n", x,y,z,a,b,c);
    // Compute absolute travel distance for each axis:
    dx = fabs(x - canon.endPoint.x);
    dy = fabs(y - canon.endPoint.y);
    dz = fabs(z - canon.endPoint.z);
    da = fabs(a - canon.endPoint.a);
    if (da > 180.0)
    {
        da = fabs(da - 360.0);
    }
    db = fabs(b - canon.endPoint.b);
    if (db > 180.0)
    {
        db = fabs(db - 360.0);
    }
    dc = fabs(c - canon.endPoint.c);
    if (dc > 180.0)
    {
        dc = fabs(dc - 360.0);
    }
    du = fabs(u - canon.endPoint.u);
    dv = fabs(v - canon.endPoint.v);
    dw = fabs(w - canon.endPoint.w);

    if (!axis_valid(0) || dx < tiny)
        dx = 0.0;
    if (!axis_valid(1) || dy < tiny)
        dy = 0.0;
    if (!axis_valid(2) || dz < tiny)
        dz = 0.0;
    if (!axis_valid(3) || da < tiny)
        da = 0.0;
    if (!axis_valid(4) || db < tiny)
        db = 0.0;
    if (!axis_valid(5) || dc < tiny)
        dc = 0.0;
    if (!axis_valid(6) || du < tiny)
        du = 0.0;
    if (!axis_valid(7) || dv < tiny)
        dv = 0.0;
    if (!axis_valid(8) || dw < tiny)
        dw = 0.0;

    // rcs_print("\n\ngetStraightVelocity dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g\n\n",
    //            dx, dy, dz, da, db, dc, du, dv, dw);

    // Figure out what kind of move we're making:
    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && du <= 0.0 && dv <= 0.0 && dw <= 0.0)
    {
        canon.cartesian_move = 0;
    }
    else
    {
        canon.cartesian_move = 1;
    }
    if (da <= 0.0 && db <= 0.0 && dc <= 0.0)
    {
        canon.angular_move = 0;
    }
    else
    {
        canon.angular_move = 1;
    }
    if (dx < Q_FUZZ && dy < Q_FUZZ && dz < Q_FUZZ && da < Q_FUZZ && db < Q_FUZZ && dc < Q_FUZZ)
    {
        *alread_inpos = 1;
    }

    // Pure linear move:
    if (canon.cartesian_move && !canon.angular_move)
    {
        tx = dx ? fabs(dx / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(0))) : 0.0;
        ty = dy ? fabs(dy / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(1))) : 0.0;
        tz = dz ? fabs(dz / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(2))) : 0.0;
        tu = du ? fabs(du / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(6))) : 0.0;
        tv = dv ? fabs(dv / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(7))) : 0.0;
        tw = dw ? fabs(dw / FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(8))) : 0.0;
        out.tmax = MAX3(tx, ty, tz);
        out.tmax = MAX4(tu, tv, tw, out.tmax);

        if (dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax <= 0.0)
        {
            out.vel = canon.linearFeedRate;
        }
        else
        {
            out.vel = out.dtot / out.tmax;
            if (out.vel > FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel()))
            {
                out.vel = FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel());
            }
        }
    }
    // Pure angular move:
    else if (!canon.cartesian_move && canon.angular_move)
    {
        ta = da ? fabs(da / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3))) : 0.0;
        tb = db ? fabs(db / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(4))) : 0.0;
        tc = dc ? fabs(dc / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(5))) : 0.0;
        out.tmax = MAX3(ta, tb, tc);
        out.axis = USING_A;
        out.dtot = sqrt(da * da + db * db + dc * dc);
        if (out.tmax <= 0.0)
        {
            out.vel = canon.angularFeedRate;
        }
        else
        {
            out.vel = out.dtot / out.tmax;
        }
    }
    // Combination angular and linear move:
    else if (canon.cartesian_move && canon.angular_move)
    {
        out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        // out.tmax = out.dtot/fabs(vel);
        double triangle_vel = sqrt(acc * out.dtot);
        if (triangle_vel <= vel)
        {
            if (triangle_vel != 0.0)
            {
                out.tmax = (2 * out.dtot) / triangle_vel;
            }
            else
            {
                out.tmax = (2 * out.dtot) / CART_FUZZ;
            }
        }
        else
        {
            double t1, t2, t3;
            if (acc != 0.0)
            {
                t1 = vel / acc;
                t3 = vel / acc;
            }
            else
            {
                t1 = vel / CART_FUZZ;
                t3 = vel / CART_FUZZ;
            }
            if (vel != 0.0)
            {
                t2 = (out.dtot - (0.5 * (t1 + t3) * vel)) / vel;
            }
            else
            {
                t2 = (out.dtot - (0.5 * (t1 + t3) * vel)) / CART_FUZZ;
            }
            out.tmax = t1 + t2 + t3;
            // rcs_print("t1 = %f, t2 = %f, t3 = %f, acc = %f, vel = %f, triangle_vel = %f\n", t1, t2, t3, acc, vel, triangle_vel);
        }
        // rcs_print("new: out.tmax=%f, out.dtot=%f, fabs(vel)=%f\n", out.tmax, out.dtot, fabs(vel));

        // rpy to qurternion
        PmRpy rpyTarget, rpyCurr;
        PmQuaternion quatTarget, quatCurr, quatDelt;
        rpyTarget.y = a * PM_PI / 180.0;
        rpyTarget.p = b * PM_PI / 180.0;
        rpyTarget.r = c * PM_PI / 180.0;
        rpyCurr.y = canon.endPoint.a * PM_PI / 180.0;
        rpyCurr.p = canon.endPoint.b * PM_PI / 180.0;
        rpyCurr.r = canon.endPoint.c * PM_PI / 180.0;
        pmRpyQuatConvert(&rpyTarget, &quatTarget);
        pmRpyQuatConvert(&rpyCurr, &quatCurr);
        pmQuatInv(&quatCurr, &quatCurr);
        pmQuatQuatMult(&quatTarget, &quatCurr, &quatDelt);
        double deltAngle = fabs(2 * acos(quatDelt.s) * 180.0 / PM_PI);
        rcs_print(" ######################################## [getStraightVelocity_movl]: deltAngle = %f, quatDelt = %f %f %f %f\n",
                  deltAngle,
                  quatDelt.s,
                  quatDelt.x,
                  quatDelt.y,
                  quatDelt.z);
        if (deltAngle)
        {
            double acc_angle = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3));
            double vel_angle = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3));
            double triangle_vel_angle = sqrt(acc_angle * deltAngle);
            if (triangle_vel_angle <= vel_angle)
            {
                if (triangle_vel_angle != 0.0)
                {
                    ta = (2 * deltAngle) / triangle_vel_angle;
                }
                else
                {
                    ta = (2 * deltAngle) / CART_FUZZ;
                }
            }
            else
            {
                double t1_angle, t2_angle, t3_angle;
                if (acc_angle != 0.0)
                {
                    t1_angle = vel_angle / acc_angle;
                    t3_angle = vel_angle / acc_angle;
                }
                else
                {
                    t1_angle = vel_angle / CART_FUZZ;
                    t3_angle = vel_angle / CART_FUZZ;
                }
                if (vel_angle != 0.0)
                {
                    t2_angle = (deltAngle - (0.5 * (t1_angle + t3_angle) * vel_angle)) / vel_angle;
                }
                else
                {
                    t2_angle = (deltAngle - (0.5 * (t1_angle + t3_angle) * vel_angle)) / CART_FUZZ;
                }
                ta = t1_angle + t2_angle + t3_angle;
            }
        }
        else
        {
            ta = 0.0;
        }
        // ta = deltAngle ? fabs(deltAngle / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3))) : 0.0;
        // rcs_print("new: quatDelt.s = %f, deltAngle = %f, ta = %f, robot_motion_task->zucAxisGetMaxVelocity(3) = %f\n", quatDelt.s, deltAngle, ta, robot_motion_task->zucAxisGetMaxVelocity(3));
        if (out.tmax < ta)
        {
            out.tmax = ta;
            out.axis = USING_A;
            out.dtot = deltAngle;
        }
        else
        {
            out.axis = USING_X;
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        }

        // ta = da? fabs(da / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3))): 0.0;
        // tb = db? fabs(db / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(4))): 0.0;
        // tc = dc? fabs(dc / FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(5))): 0.0;

        // if(out.tmax < ta)
        // {
        //     out.tmax = ta;
        //     out.axis = USING_A;
        // }
        // if(out.tmax < tb)
        // {
        //     out.tmax = tb;
        //     out.axis = USING_B;
        // }
        // if(out.tmax < tc)
        // {
        //     out.tmax = tc;
        //     out.axis = USING_C;
        // }

        // if(out.axis > USING_X)
        // {
        //     out.dtot = sqrt(da * da + db * db + dc * dc);
        // }
        // else
        // {
        //     out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        // }

        if (out.tmax <= 0.0)
        {
            out.vel = canon.linearFeedRate;
        }
        else
        {
            // out.vel = out.dtot / out.tmax;
            out.vel = vel;
            if (USING_X == out.axis)
            {
                if (out.vel > FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel()))
                {
                    out.vel = FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel());
                }
            }
        }
    }
    if (debug_velacc)
        printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel);
    return out;
}

static VelData getStraightVelocity(CANON_POSITION pos)
{
    return getStraightVelocity(pos.x, pos.y, pos.z, pos.a, pos.b, pos.c, pos.u, pos.v, pos.w, canon.linearFeedRate);
}

#include <vector>
struct pt
{
    double x, y, z, a, b, c, u, v, w;
    int line_no;
};

static std::vector<struct pt> chained_points;

static void flush_segments(void)
{
    if (chained_points.empty())
        return;

    struct pt& pos = chained_points.back();

    double x = pos.x, y = pos.y, z = pos.z;
    double a = pos.a, b = pos.b, c = pos.c;
    double u = pos.u, v = pos.v, w = pos.w;

    int line_no = pos.line_no;

#ifdef SHOW_JOINED_SEGMENTS
    for (unsigned int i = 0; i != chained_points.size(); i++) { printf("."); }
    printf("\n");
#endif

    VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w, canon.linearFeedRate);
    double vel = linedata.vel;

    if (canon.cartesian_move && !canon.angular_move)
    {
        if (vel > canon.linearFeedRate)
        {
            vel = canon.linearFeedRate;
        }
    }
    else if (!canon.cartesian_move && canon.angular_move)
    {
        if (vel > canon.angularFeedRate)
        {
            vel = canon.angularFeedRate;
        }
    }
    else if (canon.cartesian_move && canon.angular_move)
    {
        if (vel > canon.linearFeedRate)
        {
            vel = canon.linearFeedRate;
        }
    }

    ZUC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.feed_mode = canon.feed_mode;

    // now x, y, z, and b are in absolute mm or degree units
    linearMoveMsg.end.tran.x = TO_EXT_LEN(x);
    linearMoveMsg.end.tran.y = TO_EXT_LEN(y);
    linearMoveMsg.end.tran.z = TO_EXT_LEN(z);

    linearMoveMsg.end.u = TO_EXT_LEN(u);
    linearMoveMsg.end.v = TO_EXT_LEN(v);
    linearMoveMsg.end.w = TO_EXT_LEN(w);

    // fill in the orientation
    linearMoveMsg.end.a = TO_EXT_ANG(a);
    linearMoveMsg.end.b = TO_EXT_ANG(b);
    linearMoveMsg.end.c = TO_EXT_ANG(c);

    linearMoveMsg.vel = toExtVel(vel);
    linearMoveMsg.ini_maxvel = toExtVel(linedata.vel);
    AccelData lineaccdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);
    double acc = lineaccdata.acc;
    linearMoveMsg.acc = toExtAcc(acc);

    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return;
    }
    linearMoveMsg.jerk = robot_motion_task->zucTrajGetMaxCarteJerk();
    linearMoveMsg.type = ZUC_MOTION_TYPE_FEED;
    linearMoveMsg.indexrotary = -1;
    if ((vel && acc) || canon.synched)
    {
        interp_list.set_line_number(line_no);
        interp_list.append(linearMoveMsg);
    }
    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);

    chained_points.clear();
}

static void get_last_pos(double& lx, double& ly, double& lz)
{
    if (chained_points.empty())
    {
        lx = canon.endPoint.x;
        ly = canon.endPoint.y;
        lz = canon.endPoint.z;
    }
    else
    {
        struct pt& pos = chained_points.back();
        lx = pos.x;
        ly = pos.y;
        lz = pos.z;
    }
}

static bool linkable(double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    struct pt& pos = chained_points.back();
    if (canon.motionMode != CANON_CONTINUOUS || canon.naivecamTolerance == 0)
        return false;
    //FIXME make this length controlled elsewhere?
    if (chained_points.size() > 100)
        return false;

    //If ABCUVW motion, then the tangent calculation fails?
    // TODO is there a fundamental reason that we can't handle 9D motion here?
    if (a != pos.a)
        return false;
    if (b != pos.b)
        return false;
    if (c != pos.c)
        return false;
    if (u != pos.u)
        return false;
    if (v != pos.v)
        return false;
    if (w != pos.w)
        return false;

    if (x == canon.endPoint.x && y == canon.endPoint.y && z == canon.endPoint.z)
        return false;

    for (std::vector<struct pt>::iterator it = chained_points.begin(); it != chained_points.end(); it++)
    {
        PM_CARTESIAN M(x - canon.endPoint.x, y - canon.endPoint.y, z - canon.endPoint.z), B(canon.endPoint.x, canon.endPoint.y, canon.endPoint.z),
            P(it->x, it->y, it->z);
        double t0 = dot(M, P - B) / dot(M, M);
        if (t0 < 0)
            t0 = 0;
        if (t0 > 1)
            t0 = 1;

        double D = mag(P - (B + t0 * M));
        if (D > canon.naivecamTolerance)
            return false;
    }
    return true;
}

static void see_segment(int line_number, double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    bool changed_abc = (a != canon.endPoint.a) || (b != canon.endPoint.b) || (c != canon.endPoint.c);

    bool changed_uvw = (u != canon.endPoint.u) || (v != canon.endPoint.v) || (w != canon.endPoint.w);

    if (!chained_points.empty() && !linkable(x, y, z, a, b, c, u, v, w))
    {
        rcs_print("see_segment: flush_segments() 1\n");
        flush_segments();
    }
    pt pos = {x, y, z, a, b, c, u, v, w, line_number};
    chained_points.push_back(pos);

    rcs_print("see_segment: chained_points.push_back(%f, %f, %f, %f, %f, %f)!\n", x, y, z, a, b, c);

    if (changed_abc || changed_uvw)
    {
        rcs_print("see_segment: flush_segments() 2\n");
        flush_segments();
    }
}

void FINISH() { flush_segments(); }

void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    double vel, acc;

    flush_segments();

    ZUC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.feed_mode = 0;
    if (canon.rotary_unlock_for_traverse != -1)
        linearMoveMsg.type = ZUC_MOTION_TYPE_INDEXROTARY;
    else
        linearMoveMsg.type = ZUC_MOTION_TYPE_TRAVERSE;

    from_prog(x, y, z, a, b, c, u, v, w);
    rotate_and_offset_pos(x, y, z, a, b, c, u, v, w);

    VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w, canon.linearFeedRate);
    AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);

    vel = veldata.vel;
    acc = accdata.acc;
    linearMoveMsg.using_abc = veldata.axis > USING_X ? 1 : 0;
    linearMoveMsg.end = to_ext_pose(x, y, z, a, b, c, u, v, w);
    linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel);

    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return;
    }
    if (linearMoveMsg.using_abc)
    {
        linearMoveMsg.acc = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3));
        // rcs_print("-------STRAIGHT_TRAVERSE---------: using_abc = %d  linearMoveMsg.acc = %f  \n",linearMoveMsg.using_abc,linearMoveMsg.acc);
    }
    else
    {
        linearMoveMsg.acc = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(0));
        // rcs_print("-------STRAIGHT_TRAVERSE---------: using_abc = %d  linearMoveMsg.acc = %f  \n",linearMoveMsg.using_abc,linearMoveMsg.acc);
    }

    linearMoveMsg.indexrotary = canon.rotary_unlock_for_traverse;

    int old_feed_mode = canon.feed_mode;
    if (canon.feed_mode)
        STOP_SPEED_FEED_SYNCH();

    if (vel && acc)
    {
        interp_list.set_line_number(line_number);
        interp_list.append(linearMoveMsg);
    }

    if (old_feed_mode)
        START_SPEED_FEED_SYNCH(canon.linearFeedRate, 1);

    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);
}

void MOVC_TRAVERSE(int lineno,
                   int set_planner_type,
                   int planner_type,
                   double* x,
                   double* y,
                   double* z,
                   double* a,
                   double* b,
                   double* c,
                   double vel,
                   double acceleration,
                   double jerk,
                   double count,
                   int di_type,
                   int di_index,
                   int di_state,
                   int circlemode)
{
    flush_segments();
    ZUC_TRAJ_MOVC cMoveMsg;
    cMoveMsg.circlemode = circlemode;
    cMoveMsg.di_type = di_type;
    cMoveMsg.di_index = di_index;
    cMoveMsg.di_state = di_state;
    cMoveMsg.midPoint.tran.x = x[0];
    cMoveMsg.midPoint.tran.y = y[0];
    cMoveMsg.midPoint.tran.z = z[0];
    cMoveMsg.midPoint.a = a[0];
    cMoveMsg.midPoint.b = b[0];
    cMoveMsg.midPoint.c = c[0];

    cMoveMsg.endPoint.tran.x = x[1];
    cMoveMsg.endPoint.tran.y = y[1];
    cMoveMsg.endPoint.tran.z = z[1];
    cMoveMsg.endPoint.a = a[1];
    cMoveMsg.endPoint.b = b[1];
    cMoveMsg.endPoint.c = c[1];

    cMoveMsg.circle_count = count;
    //计算速度、旋转速度所用的时间。
    double ms_x;
    double ms_y;
    double ms_z;

    double me_x;
    double me_y;
    double me_z;

    double se_x;
    double se_y;
    double se_z;
    double ms = 0.0, me = 0.0, se = 0.0;

    ms_x = x[0] - canon.endPoint.x;
    ms_y = y[0] - canon.endPoint.y;
    ms_z = z[0] - canon.endPoint.z;

    me_x = x[1] - x[0];
    me_y = y[1] - y[0];
    me_z = z[1] - z[0];

    se_x = x[1] - canon.endPoint.x;
    se_y = y[1] - canon.endPoint.y;
    se_z = z[1] - canon.endPoint.z;

    // rcs_print("-------MOVC_TRAVERSE---------:  se_x =%f  se_y=%f  se_z  = %f   \n",se_x,se_y,se_z);
    // rcs_print("-------MOVC_TRAVERSE---------:  endPoint.x =%f  endPoint.y=%f  endPoint.z  = %f   \n",canon.endPoint.x,canon.endPoint.y,canon.endPoint.z);

    // double theta, delt_ori;
    // double strLong;
    // double me_ms_dot;
    ms = sqrt(ms_x * ms_x + ms_y * ms_y + ms_z * ms_z);
    me = sqrt(me_x * me_x + me_y * me_y + me_z * me_z);
    se = sqrt(se_x * se_x + se_y * se_y + se_z * se_z);
    // me_ms_dot = me_x*ms_x + me_y*ms_y + me_z*ms_z;

    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return;
    }

    if ((ms > tiny) && (me > tiny) && (se > tiny))
    {
        // theta = acos(me_ms_dot/ms/me);
        /*calculate the orientation increment*/
        // PmRpy rpyTarget, rpyCurr;
        // PmQuaternion quatTarget, quatCurr, quatDelt;
        // rpyTarget.y = a[1] * PM_PI / 180.0;
        // rpyTarget.p = b[1] * PM_PI / 180.0;
        // rpyTarget.r = c[1] * PM_PI / 180.0;
        // rpyCurr.y = canon.endPoint.a * PM_PI / 180.0;
        // rpyCurr.p = canon.endPoint.b * PM_PI / 180.0;
        // rpyCurr.r = canon.endPoint.c * PM_PI / 180.0;
        // pmRpyQuatConvert(&rpyTarget, &quatTarget);
        // pmRpyQuatConvert(&rpyCurr, &quatCurr);
        // pmQuatInv(&quatCurr, &quatCurr);
        // pmQuatQuatMult(&quatTarget, &quatCurr, &quatDelt);
        // delt_ori = fabs(2*acos(quatDelt.s)* 180.0 / PM_PI);
    }
    else
    {
        cMoveMsg.vel = vel;
        cMoveMsg.using_abc = 0;
        cMoveMsg.ini_maxvel = vel;
        cMoveMsg.acc = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3));
        if (vel)
        {
            interp_list.set_line_number(lineno);
            interp_list.append(cMoveMsg);
        }
        canonUpdateEndPoint(x[1], y[1], z[1], a[1], b[1], c[1], 0.0, 0.0, 0.0);
        return;
    }

    // strLong = se/sin(theta)*theta/2;
    // double dtr;
    // double da,db,dc;
    // double ta,tb,tc;
    // da = fabs(canon.endPoint.a- a[1]);
    // db = fabs(canon.endPoint.b- b[1]);
    // dc = fabs(canon.endPoint.c- c[1]);

    // ta = da/FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3));
    // tb = db/FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(4));
    // tc = dc/FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(5));

    // dtr = MAX3(ta, tb, tc);
    // rcs_print("-------MOVC_TRAVERSE---------:  strLong =%f  vel=%f  dtl = %f  dtr = %f  \n",strLong,vel,dtl,dtr);
    //if(dtl >= dtr)
    if (1)
    {
        //使用线速度。
        cMoveMsg.vel = vel;
        cMoveMsg.using_abc = 0;
        cMoveMsg.ini_maxvel = vel;
    }
    else
    {
        cMoveMsg.vel = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3));  // wont use oritation as main planner
        cMoveMsg.ini_maxvel = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3));
        cMoveMsg.using_abc = 1;
    }
    //获取加速度
    if (cMoveMsg.using_abc)
    {
        cMoveMsg.acc = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3));
        // rcs_print("-------MOVC_TRAVERSE---------: using_abc = %d  cMoveMsg.acc = %f  \n",cMoveMsg.using_abc,cMoveMsg.acc);
    }
    else
    {
        cMoveMsg.acc = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(0));
        if (acceleration > 0 && acceleration < cMoveMsg.acc)
        {
            cMoveMsg.acc = acceleration;
        }
        // rcs_print("-------MOVC_TRAVERSE---------: using_abc = %d  cMoveMsg.acc = %f  acceleration=%f \n",cMoveMsg.using_abc,cMoveMsg.acc,acceleration);
    }
    if (set_planner_type != -1)
    {
        cMoveMsg.planner_type = set_planner_type;
    }
    else
    {
        cMoveMsg.planner_type = planner_type;
    }
    if (cMoveMsg.planner_type == S_PLANNER)
    {
        cMoveMsg.jerk = robot_motion_task->zucTrajGetMaxCarteJerk();
    }
    else
    {
        cMoveMsg.jerk = 0.0;
    }

    if (vel)
    {
        interp_list.set_line_number(lineno);
        interp_list.append(cMoveMsg);
    }
    canonUpdateEndPoint(x[1], y[1], z[1], a[1], b[1], c[1], 0.0, 0.0, 0.0);
}

void MOVL_TRAVERSE(int lineno,
                   int set_planner_type,
                   int planner_type,
                   double x,
                   double y,
                   double z,
                   double a,
                   double b,
                   double c,
                   double u,
                   double v,
                   double w,
                   double vel,
                   double acceleration,
                   double jerk,
                   double ori_velocity,
                   double ori_acceleration,
                   int di_type,
                   int di_index,
                   int di_state,
                   int robot_id)
{
    double cmd_vel, cmd_acc;
    double cmd_ori_vel, cmd_ori_acc;
    flush_segments();
    cmd_vel = vel;
    cmd_acc = acceleration;
    cmd_ori_vel = ori_velocity;
    cmd_ori_acc = ori_acceleration;
    ZUC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.di_type = di_type;
    linearMoveMsg.di_index = di_index;
    linearMoveMsg.di_state = di_state;

    linearMoveMsg.feed_mode = 0;
    if (canon.rotary_unlock_for_traverse != -1)
        linearMoveMsg.type = ZUC_MOTION_TYPE_INDEXROTARY;
    else
        linearMoveMsg.type = ZUC_MOTION_TYPE_TRAVERSE;
    from_prog(x, y, z, a, b, c, u, v, w);
    rotate_and_offset_pos(x, y, z, a, b, c, u, v, w);
    linearMoveMsg.end = to_ext_pose(x, y, z, a, b, c, u, v, w);

    // VelData veldata = getStraightVelocity_movl(x, y, z, a, b, c, u, v, w,vel,acceleration);
    // // VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w,vel);
    // if(veldata.axis>USING_X)
    // {
    //     double maxvel = vel;
    //     maxvel = robot_motion_task->zucAxisGetMaxVelocity(3);// changed by sxl robot_motion_task->zucAxisGetMaxAcceleration---> robot_motion_task->zucAxisGetMaxVelocity
    // 	linearMoveMsg.vel = linearMoveMsg.ini_maxvel = maxvel;
    // 	linearMoveMsg.using_abc = 1;
    // }
    // else
    // {
    // 	vel = vel < toExtVel(veldata.vel) ? vel : toExtVel(veldata.vel);
    if (robot_id < 0 || robot_id >= (int)mot::al::MotionProxy::instance().robot_cnt())
    {
        return;
    }
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(robot_id);
    if (!robot_motion_task.get())
    {
        return;
    }
    linearMoveMsg.robot_id = robot_id;
    vel = vel < FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel()) ? vel : FROM_EXT_LEN(robot_motion_task->zucTrajGetMaxVel());
    vel = vel < FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(0)) ? vel : FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(0));
    linearMoveMsg.vel = linearMoveMsg.ini_maxvel = vel;
    linearMoveMsg.using_abc = 0;
    // }

    ////*limit orientation velocity and acceleration
    linearMoveMsg.ori_vel = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(3));
    if (cmd_ori_vel > 0 && cmd_ori_vel < linearMoveMsg.ori_vel)
    {
        linearMoveMsg.ori_vel = cmd_ori_vel;
    }
    linearMoveMsg.ori_acc = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(3));
    if (cmd_ori_acc > 0 && cmd_ori_acc < linearMoveMsg.ori_acc)
    {
        linearMoveMsg.ori_acc = cmd_ori_acc;
    }
    // vel = vel < toExtVel(veldata.vel) ? vel : toExtVel(veldata.vel);
    // linearMoveMsg.vel = linearMoveMsg.ini_maxvel = vel;
    // linearMoveMsg.using_abc = veldata.axis>USING_X?1:0;
    // rcs_print("lineno = %d, linearMoveMsg.vel = %f using_abc = %d\n", lineno, linearMoveMsg.vel,linearMoveMsg.using_abc);

    // AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);
    // linearMoveMsg.acc = toExtAcc(accdata.acc);
    if (linearMoveMsg.using_abc)
    {
        linearMoveMsg.acc = FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3));
        // rcs_print("-------MOVL_TRAVERSE---------: using_abc = %d  linearMoveMsg.acc = %f  \n",linearMoveMsg.using_abc,linearMoveMsg.acc);
    }
    else
    {
        linearMoveMsg.acc = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(0));
        if (acceleration > 0 && acceleration < linearMoveMsg.acc)
        {
            linearMoveMsg.acc = acceleration;
        }
        //rcs_print("-------MOVL_TRAVERSE---------: using_abc = %d  linearMoveMsg.acc = %f  vel=%f \n",linearMoveMsg.using_abc,linearMoveMsg.acc,linearMoveMsg.vel);
    }
    //rcs_print("-------MOVL_TRAVERSE---------: end = %f  %f %f  \n",linearMoveMsg.end.tran.x,linearMoveMsg.end.tran.y,linearMoveMsg.end.tran.z);
    linearMoveMsg.indexrotary = canon.rotary_unlock_for_traverse;

    if (set_planner_type != -1)
    {
        linearMoveMsg.planner_type = set_planner_type;
    }
    else
    {
        linearMoveMsg.planner_type = planner_type;
    }
    if (linearMoveMsg.planner_type == S_PLANNER)
    {
        linearMoveMsg.jerk = robot_motion_task->zucTrajGetMaxCarteJerk();
    }
    else
    {
        linearMoveMsg.jerk = 0.0;
    }
    // rcs_print("Movl_traverse: movl jerk = %lf!\n",linearMoveMsg.jerk);

    int old_feed_mode = canon.feed_mode;
    if (canon.feed_mode)
        STOP_SPEED_FEED_SYNCH();

    if (vel)
    {
        interp_list.set_line_number(lineno);
        interp_list.append(linearMoveMsg);
    }
    else
    {
        linearMoveMsg.vel = linearMoveMsg.ini_maxvel = cmd_vel;
        linearMoveMsg.using_abc = 0;
        linearMoveMsg.acc = cmd_acc;
        interp_list.set_line_number(lineno);
        interp_list.append(linearMoveMsg);
    }
    if (old_feed_mode)
        START_SPEED_FEED_SYNCH(canon.linearFeedRate, 1);

    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);
}

void JOINT_TRAVERSE(int lineno,
                    int set_planner_type,
                    int planner_type,
                    double* jointPos,
                    double* currentJointPos,
                    double vel,
                    double acceleration,
                    double jerk,
                    int di_type,
                    int di_index,
                    int di_state,
                    ZucPose* toolOffset,
                    ZucPose* userOffset,
                    int robot_id)
{
    // rcs_print("[JOINT_TRAVERSE] ---> acceleration = %f\n", acceleration);
    flush_segments();
    // double deltaD[ZUCMOT_MAX_JOINTS];
    // double deltaT[ZUCMOT_MAX_JOINTS];
    // double maxTime = 0.0;
    int maxTimeJointNum = 0;
    double maxTimeVel = vel;
    // double realVel;
    // for (int i = 0; i < ZUCMOT_MAX_JOINTS; i++)
    // {
    //     deltaD[i] = fabs(currentJointPos[i] - jointPos[i]);
    //     double jointMaxVel = zucJointGetMaxVelocity(i);
    //     realVel = (vel < jointMaxVel) ? vel : jointMaxVel;
    //     deltaT[i] = deltaD[i] / realVel;
    //     // rcs_print(" currentJointPos = %f, jointPos = %f, deltaD[%d] = %f, realVel = %f, deltaT[i] = %f\n", currentJointPos[i], jointPos[i], i, deltaD[i], realVel, deltaT[i]);
    //     if ((deltaT[i] > maxTime) && deltaD[i] >= 1e-3)
    //     {
    //         maxTime = deltaT[i];
    //         maxTimeJointNum = i;
    //         maxTimeVel = realVel;
    //         // rcs_print("pivot joint: %d, tarPos = %f, curPos = %f, finalvel = %f, maxVel = %f\n", i, jointPos[i], currentJointPos[i], realVel, jointMaxVel);
    //     }
    // }

    ZUC_TRAJ_JOINT_MOVE jointMoveMsg;

    jointMoveMsg.feed_mode = 0;
    // int i = 0;
    // double acc = zucJointGetMaxAcceleration(0);
    // double maxvel = zucJointGetMaxVelocity(0);
    // Zucpose is actually the joint pos for movj
    jointMoveMsg.end.tran.x = jointPos[0];
    jointMoveMsg.end.tran.y = jointPos[1];
    jointMoveMsg.end.tran.z = jointPos[2];
    jointMoveMsg.end.a = jointPos[3];
    jointMoveMsg.end.b = jointPos[4];
    jointMoveMsg.end.c = jointPos[5];
    jointMoveMsg.end.u = jointPos[6];
    jointMoveMsg.end.v = 0;
    jointMoveMsg.end.w = 0;
    jointMoveMsg.di_type = di_type;
    jointMoveMsg.di_index = di_index;
    jointMoveMsg.di_state = di_state;
    jointMoveMsg.jointNum = maxTimeJointNum;

    if (robot_id < 0 || robot_id >= (int)mot::al::MotionProxy::instance().robot_cnt())
    {
        return;
    }
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(robot_id);
    if (!robot_motion_task.get())
    {
        return;
    }
    jointMoveMsg.robot_id = robot_id;
    //AccelData accdata = getStraightAcceleration(jnt1, jnt2, jnt3, jnt4, jnt5, jnt6, 0, 0, 0);

    //jointMoveMsg.acc = toExtAcc(accdata.acc);        // fix me
    // printf(" [JOINT_TRAVERSE] : acceleration = %f \n ",acceleration);
    // for(i = 0 ;i < 6;i++) {
    //     printf(" [JOINT_TRAVERSE] : zucJointGetMaxAcceleration(%d) = %f \n ",i,zucJointGetMaxAcceleration(i));
    //     if(acc > zucJointGetMaxAcceleration(i)) {
    //         acc = zucJointGetMaxAcceleration(i);
    //     }

    //     if(maxvel > zucJointGetMaxVelocity(i)) {
    //         maxvel = zucJointGetMaxVelocity(i);
    //     }
    // }
    // if(acceleration>0.0 && acceleration<acc){
    //     acc = acceleration;
    // }
    // printf(" [JOINT_TRAVERSE] : acc = %f \n ",acc);
    if (set_planner_type != -1)
    {
        jointMoveMsg.planner_type = set_planner_type;
    }
    else
    {
        jointMoveMsg.planner_type = planner_type;
    }
    if (jointMoveMsg.planner_type == S_PLANNER)
    {
        jointMoveMsg.jerk = robot_motion_task->zucTrajGetMaxJointJerk();
    }
    else
    {
        jointMoveMsg.jerk = 0.0;
    }

    jointMoveMsg.vel = maxTimeVel;
    jointMoveMsg.ini_maxvel = maxTimeVel;
    jointMoveMsg.acc = robot_motion_task->zucJointGetMaxAcceleration(maxTimeJointNum);
    // rcs_print("[JOINT_TRAVERSE] ---> jointMoveMsg.acc = %f, maxTimeJointNum = %d\n", jointMoveMsg.acc, maxTimeJointNum);
    if (jointMoveMsg.acc > acceleration)
    {
        jointMoveMsg.acc = acceleration;
    }
    jointMoveMsg.type = ZUC_MOTION_TYPE_JOINT_LINE;
    jointMoveMsg.indexrotary = -1;

    int old_feed_mode = canon.feed_mode;
    if (canon.feed_mode)
        STOP_SPEED_FEED_SYNCH();

    if ((vel && jointMoveMsg.acc))
    {
        // rcs_print(" ********************************* interp_list.append(jointMoveMsg), vel = %f, acc = %f ********************************* \n", vel, jointMoveMsg.acc);
        interp_list.set_line_number(lineno);
        interp_list.append(jointMoveMsg);
    }
    else
    {
        rcs_print(" ********************************* [JOINT_TRANVERSE] not append, vel = %f, jointMoveMsg.acc = %f ********************************* \n",
                  vel,
                  jointMoveMsg.acc);
    }

    if (old_feed_mode)
        START_SPEED_FEED_SYNCH(canon.linearFeedRate, 1);

    ZucPose updatedPos;
    double joint[6] = {jointMoveMsg.end.tran.x, jointMoveMsg.end.tran.y, jointMoveMsg.end.tran.z, jointMoveMsg.end.a, jointMoveMsg.end.b, jointMoveMsg.end.c};
    // rcs_print("canonUpdateEndPoint: joint pos: %f, %f, %f, %f, %f, %f\n", joint[0], joint[1], joint[2], joint[3], joint[4], joint[5]);
    GET_KINEMATICS_FORWARD_TOOL(joint, &updatedPos, toolOffset, userOffset);
    canonUpdateEndPoint(
        updatedPos.tran.x, updatedPos.tran.y, updatedPos.tran.z, updatedPos.a, updatedPos.b, updatedPos.c, updatedPos.u, updatedPos.v, updatedPos.w);
    // rcs_print("canonUpdateEndPoint: cart pos: %f, %f, %f, %f, %f, %f\n", updatedPos.tran.x, updatedPos.tran.y, updatedPos.tran.z, updatedPos.a, updatedPos.b, updatedPos.c);
}
void MOVS_TRAVERSE(int lineno,
                   const double* jointPos,
                   const double* a0,
                   const double* a1,
                   const double* a2,
                   const double* a3,
                   double time,
                   const ZucPose* toolOffset,
                   const ZucPose* userOffset)
{
    ZucPose updatedPos;
    double joint[6];
    ZUC_TRAJ_MOVS movsMsg;
    for (int i = 0; i < 6; i++) { joint[i] = jointPos[i]; }
    GET_KINEMATICS_FORWARD_TOOL(joint, &updatedPos, toolOffset, userOffset);
    canonUpdateEndPoint(
        updatedPos.tran.x, updatedPos.tran.y, updatedPos.tran.z, updatedPos.a, updatedPos.b, updatedPos.c, updatedPos.u, updatedPos.v, updatedPos.w);
    for (int i = 0; i < 6; i++)
    {
        movsMsg.movs_a0[i] = a0[i];
        movsMsg.movs_a1[i] = a1[i];
        movsMsg.movs_a2[i] = a2[i];
        movsMsg.movs_a3[i] = a3[i];
        movsMsg.movs_time = time;
    }
    movsMsg.type = ZUC_MOTION_TYPE_MOVS_LINE;
    interp_list.set_line_number(lineno);
    interp_list.append(movsMsg);
    return;
}

void STRAIGHT_FEED(int line_number, double x, double y, double z, double a, double b, double c, double u, double v, double w)
{
    // rcs_print("Straight_feed called, see_segment followed!\n");

    ZUC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.feed_mode = canon.feed_mode;

    from_prog(x, y, z, a, b, c, u, v, w);
    rotate_and_offset_pos(x, y, z, a, b, c, u, v, w);
    see_segment(line_number, x, y, z, a, b, c, u, v, w);
}

void RIGID_TAP(int line_number, double x, double y, double z)
{
    double ini_maxvel, acc;
    ZUC_TRAJ_RIGID_TAP rigidTapMsg;
    double unused = 0;

    from_prog(x, y, z, unused, unused, unused, unused, unused, unused);
    rotate_and_offset_pos(x, y, z, unused, unused, unused, unused, unused, unused);

    VelData veldata = getStraightVelocity(
        x, y, z, canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, canon.endPoint.u, canon.endPoint.v, canon.endPoint.w, canon.linearFeedRate);
    ini_maxvel = veldata.vel;

    AccelData accdata =
        getStraightAcceleration(x, y, z, canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, canon.endPoint.u, canon.endPoint.v, canon.endPoint.w);
    acc = accdata.acc;

    rigidTapMsg.pos = to_ext_pose(x, y, z, canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, canon.endPoint.u, canon.endPoint.v, canon.endPoint.w);

    rigidTapMsg.vel = toExtVel(ini_maxvel);
    rigidTapMsg.ini_maxvel = toExtVel(ini_maxvel);
    rigidTapMsg.acc = toExtAcc(acc);

    flush_segments();

    if (ini_maxvel && acc)
    {
        interp_list.set_line_number(line_number);
        interp_list.append(rigidTapMsg);
    }

    // don't move the endpoint because after this move, we are back where we started
}

/* Machining Attributes */

void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance)
{
    // ZUC_TRAJ_SET_TERM_COND setTermCondMsg;

    // flush_segments();

    // canon.motionMode = mode;
    // canon.motionTolerance = FROM_PROG_LEN(tolerance);

    // switch (mode)
    // {
    // case CANON_CONTINUOUS:
    //     setTermCondMsg.cond = ZUC_TRAJ_TERM_COND_BLEND;
    //     setTermCondMsg.tolerance = TO_EXT_LEN(canon.motionTolerance);
    //     break;
    // case CANON_EXACT_PATH:
    //     setTermCondMsg.cond = ZUC_TRAJ_TERM_COND_EXACT;
    //     break;

    // case CANON_EXACT_STOP:
    // default:
    //     setTermCondMsg.cond = ZUC_TRAJ_TERM_COND_STOP;
    //     break;
    // }

    // interp_list.append(setTermCondMsg);
}

void SET_NAIVECAM_TOLERANCE(double tolerance) { canon.naivecamTolerance = FROM_PROG_LEN(tolerance); }

void SELECT_PLANE(CANON_PLANE in_plane) { canon.activePlane = in_plane; }

void SET_CUTTER_RADIUS_COMPENSATION(double radius)
{
    // nothing need be done here
}

void START_CUTTER_RADIUS_COMPENSATION(int side)
{
    // nothing need be done here
}

void STOP_CUTTER_RADIUS_COMPENSATION()
{
    // nothing need be done here
}

void START_SPEED_FEED_SYNCH(double feed_per_revolution, bool velocity_mode) {}

void STOP_SPEED_FEED_SYNCH() {}

/* Machining Functions */
static double chord_deviation(double sx, double sy, double ex, double ey, double cx, double cy, int rotation, double& mx, double& my)
{
    double th1 = atan2(sy - cy, sx - cx), th2 = atan2(ey - cy, ex - cx), r = hypot(sy - cy, sx - cx), dth = th2 - th1;

    if (rotation < 0)
    {
        if (dth >= -1e-5)
            th2 -= 2 * M_PI;
        // in the edge case where atan2 gives you -pi and pi, a second iteration is needed
        // to get these in the right order
        dth = th2 - th1;
        if (dth >= -1e-5)
            th2 -= 2 * M_PI;
    }
    else
    {
        if (dth <= 1e-5)
            th2 += 2 * M_PI;
        dth = th2 - th1;
        if (dth <= 1e-5)
            th2 += 2 * M_PI;
    }

    double included = fabs(th2 - th1);
    double mid = (th2 + th1) / 2;
    mx = cx + r * cos(mid);
    my = cy + r * sin(mid);
    double dev = r * (1 - cos(included / 2));
    return dev;
}

/* Spline and NURBS additional functions; */

static double max(double a, double b)
{
    if (a < b)
        return b;
    return a;
}
static void unit(double* x, double* y)
{
    double h = hypot(*x, *y);
    if (h != 0)
    {
        *x /= h;
        *y /= h;
    }
}

static void arc(int lineno, double x0, double y0, double x1, double y1, double dx, double dy)
{
    double small = 0.000001;
    double x = x1 - x0, y = y1 - y0;
    double den = 2 * (y * dx - x * dy);
    CANON_POSITION p = unoffset_and_unrotate_pos(canon.endPoint);
    to_prog(p);
    if (fabs(den) > small)
    {
        double r = -(x * x + y * y) / den;
        double i = dy * r, j = -dx * r;
        double cx = x0 + i, cy = y0 + j;
        ARC_FEED(lineno, x1, y1, cx, cy, r < 0 ? 1 : -1, p.z, p.a, p.b, p.c, p.u, p.v, p.w);
    }
    else
    {
        STRAIGHT_FEED(lineno, x1, y1, p.z, p.a, p.b, p.c, p.u, p.v, p.w);
    }
}

static int biarc(int lineno, double p0x, double p0y, double tsx, double tsy, double p4x, double p4y, double tex, double tey, double r = 1.0)
{
    unit(&tsx, &tsy);
    unit(&tex, &tey);

    double vx = p0x - p4x, vy = p0y - p4y;
    double c = vx * vx + vy * vy;
    double b = 2 * (vx * (r * tsx + tex) + vy * (r * tsy + tey));
    double a = 2 * r * (tsx * tex + tsy * tey - 1);

    double discr = b * b - 4 * a * c;
    if (discr < 0)
        return 0;

    double disq = sqrt(discr);
    double beta1 = (-b - disq) / 2 / a;
    double beta2 = (-b + disq) / 2 / a;

    if (beta1 > 0 && beta2 > 0)
        return 0;
    double beta = max(beta1, beta2);
    double alpha = beta * r;
    double ab = alpha + beta;
    double p1x = p0x + alpha * tsx, p1y = p0y + alpha * tsy, p3x = p4x - beta * tex, p3y = p4y - beta * tey, p2x = (p1x * beta + p3x * alpha) / ab,
           p2y = (p1y * beta + p3y * alpha) / ab;
    double tmx = p3x - p2x, tmy = p3y - p2y;
    unit(&tmx, &tmy);

    arc(lineno, p0x, p0y, p2x, p2y, tsx, tsy);
    arc(lineno, p2x, p2y, p4x, p4y, tmx, tmy);
    return 1;
}

/* Canon calls */

void NURBS_FEED(int lineno, std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k)
{
    flush_segments();

    unsigned int n = nurbs_control_points.size() - 1;
    double umax = n - k + 2;
    unsigned int div = nurbs_control_points.size() * 4;
    std::vector<unsigned int> knot_vector = knot_vector_creator(n, k);
    PLANE_POINT P0, P0T, P1, P1T;

    P0 = nurbs_point(0, k, nurbs_control_points, knot_vector);
    P0T = nurbs_tangent(0, k, nurbs_control_points, knot_vector);

    for (unsigned int i = 1; i <= div; i++)
    {
        double u = umax * i / div;
        P1 = nurbs_point(u, k, nurbs_control_points, knot_vector);
        P1T = nurbs_tangent(u, k, nurbs_control_points, knot_vector);
        biarc(lineno, P0.X, P0.Y, P0T.X, P0T.Y, P1.X, P1.Y, P1T.X, P1T.Y);
        P0 = P1;
        P0T = P1T;
    }
    knot_vector.clear();
}

/**
 * Simple circular shift function for PM_CARTESIAN type.
 * Cycle around axes without changing the individual values. A circshift of -1
 * makes the X value become the new Y, Y become the Z, and Z become the new X.
 */
static PM_CARTESIAN circshift(PM_CARTESIAN& vec, int steps)
{
    int X = 0, Y = 1, Z = 2;

    int s = 3;
    // Use mod to cycle indices around by steps
    X = (X + steps + s) % s;
    Y = (Y + steps + s) % s;
    Z = (Z + steps + s) % s;
    return PM_CARTESIAN(vec[X], vec[Y], vec[Z]);
}

#if 0
static CANON_POSITION get_axis_max_velocity()
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0); if (!robot_motion_task.get()) {return -1;}
    CANON_POSITION maxvel;
    maxvel.x = axis_valid(0) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(0)) : 0.0;
    maxvel.y = axis_valid(1) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(1)) : 0.0;
    maxvel.z = axis_valid(2) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(2)) : 0.0;

    maxvel.a = axis_valid(3) ? FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(3)) : 0.0;
    maxvel.b = axis_valid(4) ? FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(4)) : 0.0;
    maxvel.c = axis_valid(5) ? FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxVelocity(5)) : 0.0;

    maxvel.u = axis_valid(6) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(6)) : 0.0;
    maxvel.v = axis_valid(7) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(7)) : 0.0;
    maxvel.w = axis_valid(8) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(8)) : 0.0;
    return maxvel;
}

static CANON_POSITION get_axis_max_acceleration()
{
    CANON_POSITION maxacc;
    maxacc.x = axis_valid(0) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(0)) : 0.0;
    maxacc.y = axis_valid(1) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(1)) : 0.0;
    maxacc.z = axis_valid(2) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(2)) : 0.0;

    maxacc.a = axis_valid(3) ? FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(3)) : 0.0;
    maxacc.b = axis_valid(4) ? FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(4)) : 0.0;
    maxacc.c = axis_valid(5) ? FROM_EXT_ANG(robot_motion_task->zucAxisGetMaxAcceleration(5)) : 0.0;

    maxacc.u = axis_valid(6) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(6)) : 0.0;
    maxacc.v = axis_valid(7) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(7)) : 0.0;
    maxacc.w = axis_valid(8) ? FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(8)) : 0.0;
    return maxacc;
}

static double axis_motion_time(const CANON_POSITION & start, const CANON_POSITION & end)
{

    CANON_POSITION disp = end - start;
    CANON_POSITION times; 
    CANON_POSITION maxvel = get_axis_max_velocity();

    canon_debug(" in axis_motion_time\n");
    // For active axes, find the time required to reach the displacement in each axis
    int ind = 0;
    for (ind = 0; ind < 9; ++ind) {
        double v = maxvel[ind];
        if (v > 0.0) {
            times[ind] = fabs(disp[ind]) / v;
        } else {
            times[ind]=0;
        }
        canon_debug("  ind = %d, maxvel = %f, disp = %f, time = %f\n", ind, v, disp[ind], times[ind]);
    }

    return times.max();
}

// NOTE: not exactly times, comment TODO
static double axis_acc_time(const CANON_POSITION & start, const CANON_POSITION & end)
{

    CANON_POSITION disp = end - start;
    CANON_POSITION times; 
    CANON_POSITION maxacc = get_axis_max_acceleration();

    for (int i = 0; i < 9; ++i) {
        double a = maxacc[i];
        if (a > 0.0) {
            times[i] = fabs(disp[i]) / a;
        } else {
            times[i]=0;
        }
    }

    return times.max();
}
#endif

void ARC_FEED(int line_number,
              double first_end,
              double second_end,
              double first_axis,
              double second_axis,
              int rotation,
              double axis_end_point,
              double a,
              double b,
              double c,
              double u,
              double v,
              double w)
{
    ZUC_TRAJ_CIRCULAR_MOVE circularMoveMsg;
    ZUC_TRAJ_LINEAR_MOVE linearMoveMsg;
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return;
    }
    canon_debug("line = %d\n", line_number);
    canon_debug("first_end = %f, second_end = %f\n", first_end, second_end);

    if (canon.activePlane == CANON_PLANE_XY && canon.motionMode == CANON_CONTINUOUS)
    {
        double mx, my;
        double lx, ly, lz;
        double unused;

        get_last_pos(lx, ly, lz);

        double fe = FROM_PROG_LEN(first_end), se = FROM_PROG_LEN(second_end), ae = FROM_PROG_LEN(axis_end_point);
        double fa = FROM_PROG_LEN(first_axis), sa = FROM_PROG_LEN(second_axis);
        rotate_and_offset_pos(fe, se, ae, unused, unused, unused, unused, unused, unused);
        rotate_and_offset_pos(fa, sa, unused, unused, unused, unused, unused, unused, unused);
        if (chord_deviation(lx, ly, fe, se, fa, sa, rotation, mx, my) < canon.naivecamTolerance)
        {
            a = FROM_PROG_ANG(a);
            b = FROM_PROG_ANG(b);
            c = FROM_PROG_ANG(c);
            u = FROM_PROG_LEN(u);
            v = FROM_PROG_LEN(v);
            w = FROM_PROG_LEN(w);

            rotate_and_offset_pos(unused, unused, unused, a, b, c, u, v, w);
            see_segment(line_number,
                        mx,
                        my,
                        (lz + ae) / 2,
                        (canon.endPoint.a + a) / 2,
                        (canon.endPoint.b + b) / 2,
                        (canon.endPoint.c + c) / 2,
                        (canon.endPoint.u + u) / 2,
                        (canon.endPoint.v + v) / 2,
                        (canon.endPoint.w + w) / 2);
            see_segment(line_number, fe, se, ae, a, b, c, u, v, w);
            return;
        }
    }

    linearMoveMsg.feed_mode = canon.feed_mode;
    circularMoveMsg.feed_mode = canon.feed_mode;
    flush_segments();

    // Start by defining 3D points for the motion end and center.
    PM_CARTESIAN end_cart(first_end, second_end, axis_end_point);
    PM_CARTESIAN center_cart(first_axis, second_axis, axis_end_point);
    PM_CARTESIAN normal_cart(0.0, 0.0, 1.0);
    PM_CARTESIAN plane_x(1.0, 0.0, 0.0);
    PM_CARTESIAN plane_y(0.0, 1.0, 0.0);

    canon_debug("start = %f %f %f\n", canonEndPoint.x, canonEndPoint.y, canonEndPoint.z);
    canon_debug("end = %f %f %f\n", end_cart.x, end_cart.y, end_cart.z);
    canon_debug("center = %f %f %f\n", center_cart.x, center_cart.y, center_cart.z);

    // Rearrange the X Y Z coordinates in the correct order based on the active plane (XY, YZ, or XZ)
    // KLUDGE CANON_PLANE is 1-indexed, hence the subtraction here to make a 0-index value
    int shift_ind = 0;
    switch (canon.activePlane)
    {
    case CANON_PLANE_XY:
        shift_ind = 0;
        break;
    case CANON_PLANE_XZ:
        shift_ind = -2;
        break;
    case CANON_PLANE_YZ:
        shift_ind = -1;
        break;
    }

    canon_debug("active plane is %d, shift_ind is %d\n", canon.activePlane, shift_ind);
    end_cart = circshift(end_cart, shift_ind);
    center_cart = circshift(center_cart, shift_ind);
    normal_cart = circshift(normal_cart, shift_ind);
    plane_x = circshift(plane_x, shift_ind);
    plane_y = circshift(plane_y, shift_ind);

    canon_debug("normal = %f %f %f\n", normal_cart.x, normal_cart.y, normal_cart.z);

    canon_debug("plane_x = %f %f %f\n", plane_x.x, plane_x.y, plane_x.z);

    canon_debug("plane_y = %f %f %f\n", plane_y.x, plane_y.y, plane_y.z);
    // Define end point in PROGRAM units and convert to CANON
    CANON_POSITION endpt(0, 0, 0, a, b, c, u, v, w);
    from_prog(endpt);

    // Store permuted XYZ end position
    from_prog_len(end_cart);
    endpt.set_xyz(end_cart);

    // Convert to CANON units
    from_prog_len(center_cart);

    // Rotate and offset the new end point to be in the same coordinate system as the current end point
    rotate_and_offset(endpt);
    rotate_and_offset_xyz(center_cart);
    rotate_and_offset_xyz(end_cart);

    canon_debug("end = %f %f %f\n", end_cart.x, end_cart.y, end_cart.z);

    canon_debug("endpt = %f %f %f\n", endpt.x, endpt.y, endpt.z);
    canon_debug("center = %f %f %f\n", center_cart.x, center_cart.y, center_cart.z);

    canon_debug("normal = %f %f %f\n", normal_cart.x, normal_cart.y, normal_cart.z);
    // Note that the "start" point is already rotated and offset

    // Define displacement vectors from center to end and center to start (3D)
    PM_CARTESIAN end_rel = end_cart - center_cart;
    PM_CARTESIAN start_rel = canon.endPoint.xyz() - center_cart;

    // Project each displacement onto the active plane
    double p_end_1 = dot(end_rel, plane_x);
    double p_end_2 = dot(end_rel, plane_y);
    double p_start_1 = dot(start_rel, plane_x);
    double p_start_2 = dot(start_rel, plane_y);

    canon_debug("planar end = %f %f\n", p_end_1, p_end_2);
    canon_debug("planar start = %f %f\n", p_start_1, p_start_2);

    canon_debug("rotation = %d\n", rotation);

    // Use the "X" (1) and Y" (2) components of the planar projections to get
    // the starting and ending angle. Note that atan2 arguments are atan2(Y,X).
    double theta_start = atan2(p_start_2, p_start_1);
    double theta_end = atan2(p_end_2, p_end_1);
    double start_radius = hypot(p_start_1, p_start_2);
    double end_radius = hypot(p_end_1, p_end_2);
    canon_debug("radius = %f\n", start_radius);
    canon_debug("raw values: theta_end = %.17e, theta_start = %.17e\n", theta_end, theta_start);

    // Correct for angle wrap so that theta_end - theta_start > 0
    int is_clockwise = rotation < 0;

    // FIXME should be a constant in canon.hh or elsewhere
    const double min_arc_angle = 1e-12;

    if (is_clockwise)
    {
        if ((theta_end + min_arc_angle) >= theta_start)
            theta_end -= M_PI * 2.0;
    }
    else
    {
        if ((theta_end - min_arc_angle) <= theta_start)
            theta_end += M_PI * 2.0;
    }

    canon_debug("theta_end = %f, theta_start = %f\n", theta_end, theta_start);

    /*
       mapping of rotation to full turns:

       rotation full COUNTERCLOCKWISE turns (- implies clockwise)
       -------- -----
              0 none (linear move)
              1 0
              2 1
             -1 0
             -2 -1 */

    // Compute the number of FULL turns in addition to the principal angle
    int full_turns = 0;
    if (rotation > 1)
    {
        full_turns = rotation - 1;
    }
    if (rotation < -1)
    {
        full_turns = rotation + 1;
    }

    double angle = theta_end - theta_start;
    double full_angle = angle + 2.0 * M_PI * (double)full_turns;
    canon_debug("angle = %f\n", angle);
    canon_debug("full turns = %d\n", full_turns);

    canon_debug("full_angle = %.17e\n", full_angle);

    //Use total angle to get spiral properties
    double spiral = end_radius - start_radius;
    double dr = spiral / fabs(full_angle);
    double min_radius = fmin(start_radius, end_radius);
    double effective_radius = sqrt(dr * dr + min_radius * min_radius);

    // KLUDGE: assumes 0,1,2 for X Y Z
    // Find normal axis
    int norm_axis_ind = (2 - shift_ind) % 3;
    // Find maximum velocities and accelerations for planar axes
    int axis1 = (norm_axis_ind + 1) % 3;
    int axis2 = (norm_axis_ind + 2) % 3;

    canon_debug("axis1 = %d, axis2 = %d\n", axis1, axis2);

    // Get planar velocity bounds
    double v1 = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(axis1));
    double v2 = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxVelocity(axis2));

    // Get planar acceleration bounds
    double a1 = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(axis1));
    double a2 = FROM_EXT_LEN(robot_motion_task->zucAxisGetMaxAcceleration(axis2));
    double v_max_axes = MIN(v1, v2);
    double a_max_axes = MIN(a1, a2);

    //FIXME allow tangential acceleration like in TP
    double a_max_normal = a_max_axes * sqrt(3.0) / 2.0;
    canon_debug("a_max_axes = %f\n", a_max_axes);

    // Compute the centripetal acceleration
    double v_max_radial = sqrt(a_max_normal * effective_radius);
    canon_debug("v_max_radial = %f\n", v_max_radial);

    // Restrict our maximum velocity in-plane if need be
    double v_max_planar = MIN(v_max_radial, v_max_axes);
    canon_debug("v_max_planar = %f\n", v_max_planar);

    // Find the equivalent maximum velocity for a linear displacement
    // This accounts for speed restrictions due to helical and other axes
    VelData veldata = getStraightVelocity(endpt);

    // Compute spiral length, first by the minimum circular arc length
    double circular_length = min_radius * fabs(full_angle);
    // Then by linear approximation of the spiral arc length function of angle
    // TODO use quadratic approximation
    double spiral_length = hypot(circular_length, spiral);

    // Compute length along normal axis and total XYZ arc length
    double axis_len = dot(end_cart - canon.endPoint.xyz(), normal_cart);
    double total_xyz_length = hypot(spiral_length, axis_len);

    // Next, compute the minimum time that we must take to complete the segment.
    // The motion computation gives us min time needed for the helical and auxiliary axes
    double t_max_motion = veldata.tmax;
    // Assumes worst case that velocity can be in any direction in the plane, so
    // we assume tangential velocity is always less than the planar velocity limit.
    // The spiral time is the min time needed to stay under the planar velocity limit.
    double t_max_spiral = spiral_length / v_max_planar;

    // Now, compute actual XYZ max velocity from this min time and the total arc length
    double t_max = fmax(t_max_motion, t_max_spiral);

    double v_max = total_xyz_length / t_max;
    canon_debug("v_max = %f\n", v_max);

    //COMPUTE ACCEL

    // Use "straight" acceleration measure to compute acceleration bounds due
    // to non-circular components (helical axis, other axes)
    AccelData accdata = getStraightAcceleration(endpt);

    double tt_max_motion = accdata.tmax;
    double tt_max_spiral = spiral_length / a_max_axes;
    double tt_max = fmax(tt_max_motion, tt_max_spiral);

    // a_max could be higher than a_max_axes, but the projection onto the
    // circle plane and helical axis will still be within limits
    double a_max = total_xyz_length / tt_max;

    // Limit velocity by maximum
    double vel = MIN(canon.linearFeedRate, v_max);
    canon_debug("current F = %f\n", currentLinearFeedRate);
    canon_debug("vel = %f\n", vel);

    canon_debug("v_max = %f\n", v_max);
    canon_debug("a_max = %f\n", a_max);

    canon.cartesian_move = 1;

    if (rotation == 0)
    {
        // linear move
        // FIXME (Rob) Am I missing something? the P word should never be zero,
        // or we wouldn't be calling ARC_FEED
        linearMoveMsg.end = to_ext_pose(endpt);
        linearMoveMsg.type = ZUC_MOTION_TYPE_ARC;
        linearMoveMsg.vel = toExtVel(vel);
        linearMoveMsg.ini_maxvel = toExtVel(v_max);
        linearMoveMsg.acc = toExtAcc(a_max);
        linearMoveMsg.indexrotary = -1;
        linearMoveMsg.jerk = robot_motion_task->zucTrajGetMaxCarteJerk();
        if (vel && a_max)
        {
            interp_list.set_line_number(line_number);
            interp_list.append(linearMoveMsg);
        }
    }
    else
    {
        circularMoveMsg.end = to_ext_pose(endpt);

        // Convert internal center and normal to external units
        circularMoveMsg.center = to_ext_len(center_cart);
        circularMoveMsg.normal = to_ext_len(normal_cart);

        if (rotation > 0)
            circularMoveMsg.turn = rotation - 1;
        else
            // reverse turn
            circularMoveMsg.turn = rotation;

        circularMoveMsg.type = ZUC_MOTION_TYPE_ARC;

        circularMoveMsg.vel = toExtVel(vel);
        circularMoveMsg.ini_maxvel = toExtVel(v_max);
        circularMoveMsg.acc = toExtAcc(a_max);
        circularMoveMsg.jerk = robot_motion_task->zucTrajGetMaxCarteJerk();
        //FIXME what happens if accel or vel is zero?
        // The end point is still updated, but nothing is added to the interp list
        // seems to be a crude way to indicate a zero length segment?
        if (vel && a_max)
        {
            interp_list.set_line_number(line_number);
            interp_list.append(circularMoveMsg);
        }
    }
    // update the end point
    canonUpdateEndPoint(endpt);
}

void DWELL(double seconds, int line_num, short sub_thread)
{
    ZUC_TRAJ_DELAY delayMsg;

    flush_segments();

    delayMsg.line_num = line_num;

    delayMsg.delay = seconds;

    // rcs_print("DWELL: time = %f, thread = %d\n", seconds, sub_thread);

    if (sub_thread)
    {
        threadInterpList[sub_thread - 1].nmlcmd_list->append(delayMsg);
    }
    else
    {
        if (line_num > 0)
        {
            interp_list.set_line_number(line_num);
        }
        interp_list.append(delayMsg);
    }
}

void ENABLE_ADMIT(int enable)
{
    ZUC_TRAJ_SET_ADMITTANCE_ENABLE enaAdmitMsg;
    flush_segments();
    enaAdmitMsg.enable = enable;
    interp_list.append(enaAdmitMsg);
}

void SETTOOL(int line_num, double* toolOffset, double* currJntPos, const ZucPose* currUserFrame)
{
    ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET settoolmsg;

    flush_segments();

    settoolmsg.robotToolOffset.tran.x = toolOffset[0];
    settoolmsg.robotToolOffset.tran.y = toolOffset[1];
    settoolmsg.robotToolOffset.tran.z = toolOffset[2];
    settoolmsg.robotToolOffset.a = toolOffset[3];
    settoolmsg.robotToolOffset.b = toolOffset[4];
    settoolmsg.robotToolOffset.c = toolOffset[5];
    settoolmsg.robotToolOffset.u = 0.0;
    settoolmsg.robotToolOffset.v = 0.0;
    settoolmsg.robotToolOffset.w = 0.0;

    interp_list.set_line_number(line_num);
    interp_list.append(settoolmsg);

    ZucPose newCanonEnd;
    // rcs_print("new toolOffset %f, %f, %f, %f, %f, %f\n", toolOffset[0], toolOffset[1], toolOffset[2], toolOffset[3], toolOffset[4], toolOffset[5]);
    // rcs_print("new joint_pos %f, %f, %f, %f, %f, %f\n", currJntPos[0], currJntPos[1], currJntPos[2], currJntPos[3], currJntPos[4], currJntPos[5]);

    GET_KINEMATICS_FORWARD_TOOL(currJntPos, &newCanonEnd, &settoolmsg.robotToolOffset, currUserFrame);
    canonUpdateEndPoint(newCanonEnd.tran.x, newCanonEnd.tran.y, newCanonEnd.tran.z, newCanonEnd.a, newCanonEnd.b, newCanonEnd.c, 0, 0, 0);
    // rcs_print("SETTOOL:canonUpdateEndPoint: %f, %f, %f, %f, %f, %f\n", newCanonEnd.tran.x, newCanonEnd.tran.y, newCanonEnd.tran.z, newCanonEnd.a, newCanonEnd.b, newCanonEnd.c);
}

void SETUSERFRAME(int line_num, double* userFrame, double* currJntPos, const ZucPose* currToolOffset)
{
    ZUC_TRAJ_SET_ROBOT_USER_FRAME setuserframemsg;

    flush_segments();

    setuserframemsg.robotUserFrame.tran.x = userFrame[0];
    setuserframemsg.robotUserFrame.tran.y = userFrame[1];
    setuserframemsg.robotUserFrame.tran.z = userFrame[2];
    setuserframemsg.robotUserFrame.a = userFrame[3];
    setuserframemsg.robotUserFrame.b = userFrame[4];
    setuserframemsg.robotUserFrame.c = userFrame[5];
    setuserframemsg.robotUserFrame.u = 0.0;
    setuserframemsg.robotUserFrame.v = 0.0;
    setuserframemsg.robotUserFrame.w = 0.0;

    interp_list.set_line_number(line_num);
    interp_list.append(setuserframemsg);

    ZucPose newCanonEnd;
    GET_KINEMATICS_FORWARD_TOOL(currJntPos, &newCanonEnd, currToolOffset, &setuserframemsg.robotUserFrame);
    canonUpdateEndPoint(newCanonEnd.tran.x, newCanonEnd.tran.y, newCanonEnd.tran.z, newCanonEnd.a, newCanonEnd.b, newCanonEnd.c, 0, 0, 0);
}

// #TODO
void SET_PAYLOAD(int line_num, Payload payload)
{
    ZUC_TRAJ_SET_PAYLOAD setpayloadmsg;
    flush_segments();
    setpayloadmsg.id = 0;
    setpayloadmsg.payload = payload;

    interp_list.set_line_number(line_num);
    interp_list.append(setpayloadmsg);
}

void SET_ADMITCTRLCONFIG(int line_num, int axis, FtConfig ftConfig, int now)
{
    ZUC_TRAJ_SET_ADMITTANCE_CONFIG setadmitconfig;
    flush_segments();
    setadmitconfig.axis = axis;
    setadmitconfig.ftConfig = ftConfig;
    setadmitconfig.now = now;
    interp_list.set_line_number(line_num);
    interp_list.append(setadmitconfig);
}

void SET_ENDFORCECOND(int line_num, int axis, EndForceCond endForceCond)
{
    ZUC_TRAJ_SET_END_FORCE_CONDITION setendforcecond;
    flush_segments();
    setendforcecond.axis = axis;
    setendforcecond.endForceCond = endForceCond;
    interp_list.set_line_number(line_num);
    interp_list.append(setendforcecond);
}

void SET_COMPLIANTCON(int line_num, double* compliantCondition)
{
    // ZUC_TRAJ_SET_FT_CONDITION setcompliantcon;
    // flush_segments();
    // for (int i = 0; i < 6; i++) { setcompliantcon.compliantCondition[i] = compliantCondition[i]; }
    // interp_list.set_line_number(line_num);
    // interp_list.append(setcompliantcon);
}

void SET_COMPLIANTTYPE(int line_num, int* typeEna)
{
    ZUC_TRAJ_SET_COMPLIANCE_ENABLE setcomplianttype;
    flush_segments();

    setcomplianttype.compliantType = typeEna[0];
    setcomplianttype.compliantEnable = typeEna[1];
    interp_list.set_line_number(line_num);
    interp_list.append(setcomplianttype);
}

void SET_VELCOMPLIANTLEVEL(int line_num, double* velCompliantCtrl)
{
    // ZUC_TRAJ_SET_VEL_COMPLIANT_CTRL setvelcompliantlevel;
    // flush_segments();
    // for (int i = 0; i < 5; i++) { setvelcompliantlevel.velCompliantCtrl[i] = velCompliantCtrl[i]; }
    // interp_list.set_line_number(line_num);
    // interp_list.append(setvelcompliantlevel);
}

void SET_FORCRCTRLFRAME(int line_num, int ftFrame)
{
    ZUC_TRAJ_SET_ADMITTANCE_FRAME forcectrlframe;
    flush_segments();

    forcectrlframe.ftFrame = ftFrame;
    interp_list.set_line_number(line_num);
    interp_list.append(forcectrlframe);
}

void DISABLE_FORCECONTROL(int line_num, int disableForceCtrl)
{
    ZUC_TRAJ_DISABLE_FORCE_CONTROL forcecontrol;
    flush_segments();
    forcecontrol.disableForceCtrl = disableForceCtrl;
    interp_list.set_line_number(line_num);
    interp_list.append(forcecontrol);
}

void SET_CLSN_LEVEL(int line_num, int clsnLevel)
{
    flush_segments();
    ZUC_SET_SERVO_PARAM servo_param_msg;
    servo_param_msg.servoParam.paramType = ServoParam::CLSN_SENSITIVITY;
    servo_param_msg.servoParam.paramVal.clsn_sensitivity = clsnLevel;

    interp_list.set_line_number(line_num);
    interp_list.append(servo_param_msg);
}

/* this is called with distances in external (machine) units */
void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, ZucPose offset, double diameter, double frontangle, double backangle, int orientation) {}

/*
  ZUC has no tool length offset. To implement it, we save it here,
  and apply it when necessary
  */

/* Misc Functions */

void CLAMP_AXIS(CANON_AXIS axis)
{ /*! \todo FIXME-- unimplemented */
}

/*
  setString and addString initializes or adds src to dst, never exceeding
  dst's maxlen chars.
*/

// static char *setString(char *dst, const char *src, int maxlen)
// {
//     dst[0] = 0;
//     strncat(dst, src, maxlen - 1);
//     dst[maxlen - 1] = 0;
//     return dst;
// }

// static char *addString(char *dst, const char *src, int maxlen)
// {
//     int dstlen = strlen(dst);
//     int srclen = strlen(src);
//     int actlen;

//     if (srclen >= maxlen - dstlen) {
// 	actlen = maxlen - dstlen - 1;
// 	dst[maxlen - 1] = 0;
//     } else {
// 	actlen = srclen;
//     }

//     strncat(dst, src, actlen);

//     return dst;
// }

void COMMENT(const char* comment)
{
    // nothing need be done here, but you can play tricks with hot comments
    return;
}

// refers to feed rate
void DISABLE_FEED_OVERRIDE()
{
    // ZUC_TRAJ_SET_FO_ENABLE set_fo_enable_msg;
    // flush_segments();

    // set_fo_enable_msg.mode = 0;
    // interp_list.append(set_fo_enable_msg);
}

void ENABLE_FEED_OVERRIDE()
{
    // ZUC_TRAJ_SET_FO_ENABLE set_fo_enable_msg;
    // flush_segments();

    // set_fo_enable_msg.mode = 1;
    // interp_list.append(set_fo_enable_msg);
}

//refers to adaptive feed override (HAL input, usefull for EDM for example)
void DISABLE_ADAPTIVE_FEED()
{
    ZUC_MOTION_ADAPTIVE zucmotAdaptiveMsg;
    flush_segments();

    zucmotAdaptiveMsg.status = 0;

    interp_list.append(zucmotAdaptiveMsg);
}

void SET_TOOL_ID(int lineno, int id)
{
    ZUC_SET_TOOL_ID zucSetToolIdMsg;
    flush_segments();

    zucSetToolIdMsg.id = id;
    interp_list.set_line_number(lineno);
    interp_list.append(zucSetToolIdMsg);
}

void SET_USER_ID(int lineno, int id)
{
    ZUC_SET_USER_ID zucSetUserIdMsg;
    flush_segments();

    zucSetUserIdMsg.id = id;
    interp_list.set_line_number(lineno);
    interp_list.append(zucSetUserIdMsg);
}

void CONVEYOR_LINEAR_ENABLE(double x, double y, double z, double pulseEquivalent, double maxDistance)
{
    ZUC_CONVEYOR_ENABLE zucConveyorEnableMsg;
    flush_segments();

    zucConveyorEnableMsg.convyr_type = 1;
    zucConveyorEnableMsg.x = x;
    zucConveyorEnableMsg.y = y;
    zucConveyorEnableMsg.z = z;
    zucConveyorEnableMsg.pulseEquivalent = pulseEquivalent;
    zucConveyorEnableMsg.maxDistance = maxDistance;
    interp_list.append(zucConveyorEnableMsg);
}

void CONVEYOR_CIRCULAR_ENABLE(double p1x,
                              double p1y,
                              double p1z,
                              double p2x,
                              double p2y,
                              double p2z,
                              double p3x,
                              double p3y,
                              double p3z,
                              double pulseEquivalent,
                              int rotate_tool,
                              double maxDistance)
{
    ZUC_CONVEYOR_ENABLE zucConveyorEnableMsg;
    flush_segments();

    zucConveyorEnableMsg.convyr_type = 2;
    zucConveyorEnableMsg.p1x = p1x;
    zucConveyorEnableMsg.p1y = p1y;
    zucConveyorEnableMsg.p1z = p1z;
    zucConveyorEnableMsg.p2x = p2x;
    zucConveyorEnableMsg.p2y = p2y;
    zucConveyorEnableMsg.p2z = p2z;
    zucConveyorEnableMsg.p3x = p3x;
    zucConveyorEnableMsg.p3y = p3y;
    zucConveyorEnableMsg.p3z = p3z;
    zucConveyorEnableMsg.pulseEquivalent = pulseEquivalent;
    zucConveyorEnableMsg.rotate_tool = rotate_tool;
    zucConveyorEnableMsg.maxDistance = maxDistance * PM_PI / 180.0;
    interp_list.append(zucConveyorEnableMsg);
}

void CONVEYOR_DISABLE(void)
{
    ZUC_CONVEYOR_DISABLE zucConveyorDisableMsg;
    flush_segments();
    // printf("-------CONVEYOR_DISABLE---------:  \n");
    interp_list.append(zucConveyorDisableMsg);

    //ZUC_TASK_PLAN_SYNCH zucTaskPlanSync;
    //interp_list.append(zucTaskPlanSync);
}

void TOPPRA_FINISH_LEFT_TC(void)
{
    ZUC_TOPPRA_FINISH_LEFT_TC zucToppraFinishLeftTcMsg;
    flush_segments();
    // printf("-------TOPPRA_FINISH_LEFT_TC---------:  \n");
    interp_list.append(zucToppraFinishLeftTcMsg);
}

void TOPPRA_FIRST_COMMAND(void)
{
    ZUC_TOPPRA_FIRST_COMMAND zucToppraFirstCommandMsg;
    flush_segments();
    // printf("-------TOPPRA_FIRST_COMMAND---------:  \n");
    interp_list.append(zucToppraFirstCommandMsg);
}

int GET_TOOL_OFFSET(int id, ZucPose* tool)
{
    if (id >= 0 && id < 16)
    {
        *tool = zucStatus->toolOffset[id];
        return 0;
    }
    return -1;
}

int GET_USER_OFFSET(int id, ZucPose* user)
{
    if (id >= 0 && id < 16)
    {
        *user = zucStatus->userOffset[id];
        return 0;
    }
    return -1;
}

int GET_TASK_EXECSTATE() { return zucStatus->task.execState; }

void TIO_UPDATE_SIGNAL(const char* sigName, float freq, short sub_thread)
{
    flush_segments();
    ZUC_MBTIO_UPDATE_SIGNAL m;
    strcpy(m.sigName, sigName);
    m.freq = freq;
    if (sub_thread)
        threadInterpList[sub_thread - 1].nmlcmd_list->append(m);
    else
        interp_list.append(m);
}

void TIO_SEND_COMMAND(int chnId, unsigned char* byteArr, int len, int crcType, short sub_thread)
{
    flush_segments();
    ZUC_MBTIO_SEND_COMMAND m;
    m.chnId = chnId;
    m.length = len;
    for (int i = 0; i < len; i++) { m.command[i] = byteArr[i]; }
    if (sub_thread)
        threadInterpList[sub_thread - 1].nmlcmd_list->append(m);
    else
        interp_list.append(m);
}

void MESSAGE(char* s)
{
    ZUC_OPERATOR_DISPLAY operator_display_msg;

    flush_segments();
    operator_display_msg.id = 0;
    strncpy(operator_display_msg.display, s, LINELEN);
    operator_display_msg.display[LINELEN - 1] = 0;
    interp_list.append(operator_display_msg);
}

static FILE* logfile = NULL;

void LOG(char* s)
{
    flush_segments();
    if (logfile)
    {
        fprintf(logfile, "%s\n", s);
        fflush(logfile);
    }
    fprintf(stderr, "LOG(%s)\n", s);
}

void LOGOPEN(char* name)
{
    if (logfile)
        fclose(logfile);
    logfile = fopen(name, "wt");
    fprintf(stderr, "LOGOPEN(%s) -> %p\n", name, logfile);
}

void LOGAPPEND(char* name)
{
    if (logfile)
        fclose(logfile);
    logfile = fopen(name, "at");
    fprintf(stderr, "LOGAPPEND(%s) -> %p\n", name, logfile);
}

void LOGCLOSE()
{
    if (logfile)
        fclose(logfile);
    logfile = NULL;
    fprintf(stderr, "LOGCLOSE()\n");
}

void PALLET_SHUTTLE()
{ /*! \todo FIXME-- unimplemented */
}

void UNCLAMP_AXIS(CANON_AXIS axis)
{ /*! \todo FIXME-- unimplemented */
}

/* Program Functions */

void PROGRAM_STOP(int threadId)
{
    /* 
       implement this as a pause. A resume will cause motion to proceed. */
    // ZUC_TASK_PLAN_PAUSE pauseMsg;

    // flush_segments();

    // interp_list.append(pauseMsg);

    ZUC_TASK_PLAN_PAUSE pauseMsg;
    flush_segments();
    if (threadId == 0)  //   mainThread
    {
        interp_list.append(pauseMsg);
    }
    else  //  subThread
    {
        threadInterpList[threadId - 1].nmlcmd_list->append(pauseMsg);
    }
}

void PROGRAM_RESUME(int threadId)
{
    ZUC_TASK_PLAN_RESUME resumeMsg;
    flush_segments();
    if (threadId == 0)  //   mainThread
    {
        interp_list.append(resumeMsg);
    }
    else  //  subThread
    {
        threadInterpList[threadId - 1].nmlcmd_list->append(resumeMsg);
    }
}

// void PROGRAM_EXIT()
// {

//     ZUC_TASK_ABORT exitMsg;

//     flush_segments();

//     interp_list.append(exitMsg);
// }

void SET_BLOCK_DELETE(bool state)
{
    canon.block_delete = state;  //state == ON, means we don't interpret lines starting with "/"
}

bool GET_BLOCK_DELETE()
{
    return canon.block_delete;  //state == ON, means we  don't interpret lines starting with "/"
}

void SET_OPTIONAL_PROGRAM_STOP(bool state)
{
    canon.optional_program_stop = state;  //state == ON, means we stop
}

bool GET_OPTIONAL_PROGRAM_STOP()
{
    return canon.optional_program_stop;  //state == ON, means we stop
}

void OPTIONAL_PROGRAM_STOP()
{
    ZUC_TASK_PLAN_OPTIONAL_STOP stopMsg;

    flush_segments();

    interp_list.append(stopMsg);
}

void PROGRAM_END(int threadId)
{
    // flush_segments();

    // ZUC_TASK_PLAN_END endMsg;

    // interp_list.append(endMsg);

    ZUC_TASK_PLAN_END endMsg;
    flush_segments();
    if (threadId == 0)  //   mainThread
    {
        interp_list.append(endMsg);
    }
    else  //  subThread
    {
        threadInterpList[threadId - 1].nmlcmd_list->append(endMsg);
    }
}

CANON_POSITION GET_EXTERNAL_ROBOT_TOOL_OFFSET()
{
    CANON_POSITION position;
    ZucPose robotToolOffset;

    robotToolOffset = zucStatus->task.robotToolOffset;
    position.x = robotToolOffset.tran.x;
    position.y = robotToolOffset.tran.y;
    position.z = robotToolOffset.tran.z;
    position.a = robotToolOffset.a;
    position.b = robotToolOffset.b;
    position.c = robotToolOffset.c;
    position.u = robotToolOffset.u;
    position.v = robotToolOffset.v;
    position.w = robotToolOffset.w;

    return position;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_XOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.x;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_YOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.y;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_ZOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.z;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_AOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.a;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_BOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.b;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_COFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.c;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_UOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.u;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_VOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.v;
}

double GET_EXTERNAL_ROBOT_TOOL_LENGTH_WOFFSET()
{
    CANON_POSITION robotToolOffset;
    robotToolOffset = GET_EXTERNAL_ROBOT_TOOL_OFFSET();
    return robotToolOffset.w;
}

/*
  INIT_CANON()
  Initialize canonical local variables to defaults
  */
void INIT_CANON()
{
    double units;

    chained_points.clear();

    // initialize locals to original values
    canon.rotary_unlock_for_traverse = -1;
    canon.css_maximum = 0.0;
    canon.css_numerator = 0.0;
    canon.feed_mode = 0;
    canon.synched = 0;

    SELECT_PLANE(CANON_PLANE_XY);
    canonUpdateEndPoint(0, 0, 0, 0, 0, 0, 0, 0, 0);
    SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0);
    SET_NAIVECAM_TOLERANCE(0);
    canon.spindleSpeed = 0.0;
    //    canon.preppedTool = 0;
    canon.optional_program_stop = ON;  //set enabled by default (previous ZUC behaviour)
    canon.block_delete = ON;           //set enabled by default (previous ZUC behaviour)
    canon.cartesian_move = 0;
    canon.angular_move = 0;
    canon.linearFeedRate = 0.0;
    canon.angularFeedRate = 0.0;
    ZERO_ZUC_POSE(canon.toolOffset);

    /* 
       to set the units, note that GET_EXTERNAL_LENGTH_UNITS() returns
       traj->linearUnits, which is already set from the .ini file in
       iniTraj(). This is a floating point number, in user units per mm. We
       can compare this against known values and set the symbolic values
       accordingly. If it doesn't match, we have an error. */
    units = GET_EXTERNAL_LENGTH_UNITS();
    if (fabs(units - 1.0 / 25.4) < 1.0e-3)
    {
        canon.lengthUnits = CANON_UNITS_INCHES;
    }
    else if (fabs(units - 1.0) < 1.0e-3)
    {
        canon.lengthUnits = CANON_UNITS_MM;
    }
    else
    {
        CANON_ERROR("non-standard length units, setting interpreter to mm");
        canon.lengthUnits = CANON_UNITS_MM;
    }
}

/* Sends error message */
void CANON_ERROR(const char* fmt, ...)
{
    va_list ap;
    ZUC_OPERATOR_ERROR operator_error_msg;

    flush_segments();

    operator_error_msg.id = 0;
    operator_error_msg.errcode = 0xFFFE;
    if (fmt != NULL)
    {
        va_start(ap, fmt);
        vsnprintf(operator_error_msg.error, sizeof(operator_error_msg.error), fmt, ap);
        va_end(ap);
    }
    else
    {
        operator_error_msg.error[0] = 0;
    }

    interp_list.append(operator_error_msg);
}

/*
  GET_EXTERNAL_TOOL_TABLE(int pocket)

  Returns the tool table structure associated with pocket. Note that
  pocket can run from 0 (by definition, the spindle), to pocket CANON_POCKETS_MAX - 1.

  Tool table is always in machine units.

  */
CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE(int pocket)
{
    CANON_TOOL_TABLE retval;
    return retval;
}

CANON_POSITION GET_EXTERNAL_POSITION()
{
    CANON_POSITION position;
    ZucPose pos;

    chained_points.clear();

    pos = zucStatus->motion.traj.position_desired;

    // first update internal record of last position
    canonUpdateEndPoint(FROM_EXT_LEN(pos.tran.x),
                        FROM_EXT_LEN(pos.tran.y),
                        FROM_EXT_LEN(pos.tran.z),
                        FROM_EXT_ANG(pos.a),
                        FROM_EXT_ANG(pos.b),
                        FROM_EXT_ANG(pos.c),
                        FROM_EXT_LEN(pos.u),
                        FROM_EXT_LEN(pos.v),
                        FROM_EXT_LEN(pos.w));

    // now calculate position in program units, for interpreter
    position = unoffset_and_unrotate_pos(canon.endPoint);
    to_prog(position);

    return position;
}

// feed rate wanted is in program units per minute
double GET_EXTERNAL_FEED_RATE()
{
    double feed;

    if (canon.feed_mode)
    {
        // We're in G95 "Units per Revolution" mode, so linearFeedRate
        // is the FPR and we should just return it, unchanged.
        feed = canon.linearFeedRate;
    }
    else
    {
        // We're in G94 "Units per Minute" mode so unhork linearFeedRate
        // before returning it, by converting from internal to program
        // units, and from "per second" to "per minute".
        feed = TO_PROG_LEN(canon.linearFeedRate);
        feed *= 60.0;
    }

    return feed;
}

// traverse rate wanted is in program units per minute
double GET_EXTERNAL_TRAVERSE_RATE()
{
    double traverse;

    // convert from external to program units
    traverse = TO_PROG_LEN(FROM_EXT_LEN(zucStatus->motion.traj.maxVelocity));

    // now convert from per-sec to per-minute
    traverse *= 60.0;

    return traverse;
}

double GET_EXTERNAL_LENGTH_UNITS(void)
{
    double u;

    u = zucStatus->motion.traj.linearUnits;

    if (u == 0)
    {
        CANON_ERROR("external length units are zero");
        return 1.0;
    }
    else
    {
        return u;
    }
}

double GET_EXTERNAL_ANGLE_UNITS(void)
{
    double u;

    u = zucStatus->motion.traj.angularUnits;
    //printf("----------GET_EXTERNAL_ANGLE_UNITS----------: zucStatus->motion.traj.angularUnits =%f \n",zucStatus->motion.traj.angularUnits);
    if (u == 0)
    {
        CANON_ERROR("external angle units are zero");
        return 1.0;
    }
    else
    {
        return u;
    }
}

int GET_EXTERNAL_POCKETS_MAX() { return CANON_POCKETS_MAX; }

char _parameter_file_name[LINELEN]; /* Not static.Driver
					   writes */

void GET_EXTERNAL_PARAMETER_FILE_NAME(char* file_name, /* string: to copy
							   file name into */
                                      int max_size)
{ /* maximum number of characters to copy */
    // Paranoid checks
    if (0 == file_name)
        return;

    if (max_size < 0)
        return;

    if (strlen(_parameter_file_name) < ((size_t)max_size))
        strcpy(file_name, _parameter_file_name);
    else
        file_name[0] = 0;
}

double GET_EXTERNAL_POSITION_X(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.x;
}

double GET_EXTERNAL_POSITION_Y(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.y;
}

double GET_EXTERNAL_POSITION_Z(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.z;
}

double GET_EXTERNAL_POSITION_A(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.a;
}

double GET_EXTERNAL_POSITION_B(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.b;
}

double GET_EXTERNAL_POSITION_C(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.c;
}

double GET_EXTERNAL_POSITION_U(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.u;
}

double GET_EXTERNAL_POSITION_V(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.v;
}

double GET_EXTERNAL_POSITION_W(void)
{
    CANON_POSITION position;
    position = GET_EXTERNAL_POSITION();
    return position.w;
}

/*************************Added by zxqi**************************/
/*calculate the forward kinematics*/
int GET_KINEMATICS_FORWARD(const double* joint,
                           ZucPose* world,
                           const unsigned long* fflags,
                           unsigned long* iflags,
                           const ZucPose* toolOffset,
                           const ZucPose* usrOffset)
{
    // base offset will not be changed online
    int ret = 0;  //kinematicsForward(toolOffset, joint, world, fflags, iflags, &zucStatus->task.base_offset, usrOffset);
    return ret;
}

/*calculate the inverse kinematics*/
int GET_KINEMATICS_INVERSE(const ZucPose* world,
                           double* joint,
                           const unsigned long* iflags,
                           unsigned long* fflags,
                           const ZucPose* toolOffset,
                           const ZucPose* usrOffset)
{
    // base offset will not be changed online
    // int ret = kinematicsInverse(toolOffset, world, joint, (KINEMATICS_INVERSE_FLAGS*)iflags, fflags, &zucStatus->task.base_offset, usrOffset);
    // rcs_print("Zuccanon.cc: Target world pos is {%f, %f, %f, %f, %f, %f}\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c);
    // rcs_print("Zuccanon.cc: Result joint pos is {%f, %f, %f, %f, %f, %f}\n", joint[0], joint[1], joint[2], joint[3], joint[4], joint[5]);
    return -1;
}

int GET_KINEMATICS_FORWARD_TOOL(const double* joint, ZucPose* world, const ZucPose* toolOffset, const ZucPose* usrOffset)
{
    int ret = 0;
    // rcs_print("Zuccanon.cc: toolOffset is {%f, %f, %f, %f, %f, %f}\n", toolOffset->tran.x, toolOffset->tran.y, toolOffset->tran.z, toolOffset->a, toolOffset->b, toolOffset->c);
    // rcs_print("Zuccanon.cc: usrOffset is {%f, %f, %f, %f, %f, %f}\n", usrOffset->tran.x, usrOffset->tran.y, usrOffset->tran.z, usrOffset->a, usrOffset->b, usrOffset->c);
    // ret = kinematicsForward(toolOffset, joint, world, &interp_fflags, &interp_iflags, &zucStatus->task.base_offset, usrOffset);
    return ret;
}

int GET_KINEMATICS_INVERSE_TOOL(const ZucPose* world, double* joint, const ZucPose* toolOffset, const ZucPose* usrOffset)
{
    int ret = 0;
    // ret = kinematicsInverse(toolOffset, world, joint, &interp_iflags, &interp_fflags, &zucStatus->task.base_offset, usrOffset);
    return ret;
}

int GET_TOOL_MOVE_POS(const ZucPose* world, const ZucPose* pos, ZucPose* out, const ZucPose* toolOffset, const ZucPose* usrOffset)
{
    getToolMovPos(world, pos, out);
    return 0;
}

int GET_MOVC_END(ZucPose* start, ZucPose const* const mid, ZucPose const* const end, double count, ZucPose* out)
{
    int ret = getMovcEnd(start, mid, end, count, out);
    canonUpdateEndPoint(out->tran.x, out->tran.y, out->tran.z, out->a, out->b, out->c, 0.0, 0.0, 0.0);
    return ret;
}

int GET_CURRENT_JOINT_POSITION(double* joint)
{
    if (joint == NULL)
        return -1;

    for (int i = 0; i < 6; ++i) joint[i] = zucStatus->motion.joint[i].output;

    // rcs_print("[GET_CURRENT_JOINT_POSITION] ---> joint = %f %f %f %f %f %f\n", joint[0], joint[1], joint[2],
    //             joint[3], joint[4], joint[5]);
    ZucPose tempPose;
    // kinematicsForward(
    //     &(zucStatus->task.robotToolOffset), joint, &tempPose, &interp_fflags, &interp_iflags, &zucStatus->task.base_offset, &zucStatus->task.robotUserOffset);
    // rcs_print("Zuccanon.cc: Joint pos is {%f, %f, %f, %f, %f, %f}\n", joint[0], joint[1], joint[2], joint[3], joint[4], joint[5]);
    // rcs_print("Zuccanon.cc: Flags updated, interp_fflags = %ld, interp_iflags = %ld\n", interp_fflags, interp_iflags);
    return 0;
}
/***********************End added by zxqi************************/

CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return canon.motionMode; }

double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return TO_PROG_LEN(canon.motionTolerance); }

CANON_UNITS GET_EXTERNAL_LENGTH_UNIT_TYPE() { return canon.lengthUnits; }

int GET_EXTERNAL_QUEUE_EMPTY(void)
{
    flush_segments();

    return zucStatus->motion.traj.queue == 0 ? 1 : 0;
}

// If the tool changer has prepped a pocket (after a Txxx command) and is
// ready to perform a tool change, return the currently prepped pocket
// number.  If the tool changer is idle (because no Txxx command has been
// run, or because an M6 tool change has completed), return -1.

int GET_EXTERNAL_FEED_OVERRIDE_ENABLE() { return zucStatus->motion.traj.feed_override_enabled; }

int GET_EXTERNAL_AXIS_MASK() { return zucStatus->motion.traj.axis_mask; }

CANON_PLANE GET_EXTERNAL_PLANE() { return canon.activePlane; }

/* returns current value of the digital input selected by index.*/
int GET_EXTERNAL_DIGITAL_INPUT(int index, int def)
{
    if ((index < 0) || (index >= ZUCMOT_MAX_DIO))
        return -1;

    if (zucStatus->task.input_timeout == 1)
        return -1;

#ifdef INPUT_DEBUG
    printf("GET_EXTERNAL_DIGITAL_INPUT called\n di[%d]=%d \n timeout=%d \n", index, zucStatus->io.synch_di[index], zucStatus->task.input_timeout);
#endif
    return (zucStatus->io.synch_di[index] != 0) ? 1 : 0;
}

double GET_EXTERNAL_ANALOG_INPUT(int index, double def)
{
/* returns current value of the analog input selected by index.*/
#ifdef INPUT_DEBUG
    printf("GET_EXTERNAL_ANALOG_INPUT called\n ai[%d]=%g \n timeout=%d \n", index, zucStatus->motion.analog_input[index], zucStatus->task.input_timeout);
#endif
    if ((index < 0) || (index >= ZUCMOT_MAX_AIO))
        return -1;

    if (zucStatus->task.input_timeout == 1)
        return -1;

    return zucStatus->io.analog_input[index];
}

USER_DEFINED_FUNCTION_TYPE USER_DEFINED_FUNCTION[USER_DEFINED_FUNCTION_NUM] = {0};

int USER_DEFINED_FUNCTION_ADD(USER_DEFINED_FUNCTION_TYPE func, int num)
{
    if (num < 0 || num >= USER_DEFINED_FUNCTION_NUM)
    {
        return -1;
    }

    USER_DEFINED_FUNCTION[num] = func;

    return 0;
}

/*! \function SET_MOTION_OUTPUT_BIT

  sets a DIO pin
  this message goes to task, then to motion which sets the DIO 
  when the first motion starts.
  The pin gets set with value 1 at the begin of motion, and stays 1 at the end of motion
  (this behaviour can be changed if needed)
  
  warning: setting more then one for a motion segment will clear out the previous ones 
  (the TP doesn't implement a queue of these), 
  use SET_AUX_OUTPUT_BIT instead, that allows to set the value right away
*/
void SET_MOTION_OUTPUT_BIT(int line_num, int ioType, int index, short sub_thread)
{
    ZUC_MOTION_SET_DOUT dout_msg;

    flush_segments();
    dout_msg.type = ioType;
    dout_msg.index = index;
    dout_msg.start = 1;  // startvalue = 1
    dout_msg.end = 1;    // endvalue = 1, means it doesn't get reset after current motion
    dout_msg.now = 0;    // not immediate, but synched with motion (goes to the TP)

    if (sub_thread)
    {
        threadInterpList[sub_thread - 1].nmlcmd_list->set_line_number(line_num);
        threadInterpList[sub_thread - 1].nmlcmd_list->append(dout_msg);
    }
    else
    {
        interp_list.set_line_number(line_num);
        interp_list.append(dout_msg);
    }

    return;
}

/*! \function CLEAR_MOTION_OUTPUT_BIT

  clears a DIO pin
  this message goes to task, then to motion which clears the DIO 
  when the first motion starts.
  The pin gets set with value 0 at the begin of motion, and stays 0 at the end of motion
  (this behaviour can be changed if needed)
  
  warning: setting more then one for a motion segment will clear out the previous ones 
  (the TP doesn't implement a queue of these), 
  use CLEAR_AUX_OUTPUT_BIT instead, that allows to set the value right away
*/
void CLEAR_MOTION_OUTPUT_BIT(int line_num, int ioType, int index, short sub_thread)
{
    ZUC_MOTION_SET_DOUT dout_msg;

    flush_segments();
    dout_msg.type = ioType;
    dout_msg.index = index;
    dout_msg.start = 0;  // startvalue = 1
    dout_msg.end = 0;    // endvalue = 0, means it stays 0 after current motion
    dout_msg.now = 0;    // not immediate, but synched with motion (goes to the TP)

    if (sub_thread)
    {
        threadInterpList[sub_thread - 1].nmlcmd_list->set_line_number(line_num);
        threadInterpList[sub_thread - 1].nmlcmd_list->append(dout_msg);
    }
    else
    {
        interp_list.set_line_number(line_num);
        interp_list.append(dout_msg);
    }

    return;
}

/*! \function SET_AUX_OUTPUT_BIT

  sets a DIO pin
  this message goes to task, then to motion which sets the DIO 
  right away.
  The pin gets set with value 1 at the begin of motion, and stays 1 at the end of motion
  (this behaviour can be changed if needed)
  you can use any number of these, as the effect is imediate  
*/
void SET_AUX_OUTPUT_BIT(int line_num, int ioType, int index, short sub_thread)
{
    ZUC_MOTION_SET_DOUT dout_msg;

    flush_segments();

    dout_msg.type = ioType;  // 0-->basic IO, 1-->tool IO
    dout_msg.index = index;
    dout_msg.start = 1;  // startvalue = 1
    dout_msg.end = 1;    // endvalue = 1, means it doesn't get reset after current motion
    dout_msg.now = 1;    // immediate, we don't care about synching for AUX

    if (sub_thread)
    {
        // rcs_print("SET_AUX_OUTPUT_BIT: index = %d, thread = %d\n", index, sub_thread);
        threadInterpList[sub_thread - 1].nmlcmd_list->append(dout_msg);
    }
    else
    {
        interp_list.set_line_number(line_num);
        interp_list.append(dout_msg);
    }

    return;
}

/*! \function CLEAR_AUX_OUTPUT_BIT

  clears a DIO pin
  this message goes to task, then to motion which clears the DIO 
  right away.
  The pin gets set with value 0 at the begin of motion, and stays 0 at the end of motion
  (this behaviour can be changed if needed)
  you can use any number of these, as the effect is imediate  
*/
void CLEAR_AUX_OUTPUT_BIT(int line_num, int ioType, int index, short sub_thread)
{
    ZUC_MOTION_SET_DOUT dout_msg;

    flush_segments();
    dout_msg.type = ioType;
    dout_msg.index = index;
    dout_msg.start = 0;  // startvalue = 1
    dout_msg.end = 0;    // endvalue = 0, means it stays 0 after current motion
    dout_msg.now = 1;    // immediate, we don't care about synching for AUX

    if (sub_thread)
    {
        // rcs_print("SET_AUX_OUTPUT_BIT: index = %d, thread = %d\n", index, sub_thread);
        threadInterpList[sub_thread - 1].nmlcmd_list->append(dout_msg);
    }
    else
    {
        interp_list.set_line_number(line_num);
        interp_list.append(dout_msg);
    }

    return;
}

/*! \function SET_MOTION_OUTPUT_VALUE

  sets a AIO value, not used by the RS274 Interp,
  not fully implemented in the motion controller either
*/
void SET_MOTION_OUTPUT_VALUE(int line_num, int ioType, int index, double value, short sub_thread)
{
    ZUC_MOTION_SET_AOUT aout_msg;

    flush_segments();
    aout_msg.type = ioType;
    aout_msg.index = index;  // which output
    aout_msg.start = value;  // start value
    aout_msg.end = value;    // end value
    aout_msg.now = 0;        // immediate=1, or synched when motion start=0

    if (sub_thread)
    {
        threadInterpList[sub_thread - 1].nmlcmd_list->set_line_number(line_num);
        threadInterpList[sub_thread - 1].nmlcmd_list->append(aout_msg);
    }
    else
    {
        interp_list.set_line_number(line_num);
        interp_list.append(aout_msg);
    }

    return;
}

/*! \function SET_AUX_OUTPUT_VALUE

  sets a AIO value, not used by the RS274 Interp,
  not fully implemented in the motion controller either
*/
void SET_AUX_OUTPUT_VALUE(int line_num, int ioType, int index, double value, short sub_thread)
{
    ZUC_MOTION_SET_AOUT aout_msg;

    flush_segments();
    aout_msg.type = ioType;
    aout_msg.index = index;  // which output
    aout_msg.start = value;  // start value
    aout_msg.end = value;    // end value
    aout_msg.now = 1;        // immediate=1, or synched when motion start=0

    if (sub_thread)
    {
        threadInterpList[sub_thread - 1].nmlcmd_list->set_line_number(line_num);
        threadInterpList[sub_thread - 1].nmlcmd_list->append(aout_msg);
    }
    else
    {
        interp_list.set_line_number(line_num);
        interp_list.append(aout_msg);
    }

    return;
}

void update_linenum(int line_num, short sub_thread)
{
    if (sub_thread)
    {
        threadInterpList[sub_thread - 1].nmlcmd_list->set_line_number(line_num);
    }
    else
    {
        interp_list.set_line_number(line_num);
    }
}

/*! \function WAIT
   program execution and interpreting is stopped until the input selected by 
   index changed to the needed state (specified by wait_type).
   Return value: either wait_type if timeout didn't occur, or -1 otherwise. */

int WAIT(int line_num,
         int index,      /* index of the motion exported input */
         int input_type, /* DIGITAL_INPUT or ANALOG_INPUT */
         int wait_type,  /* 0 - immediate, 1 - rise, 2 - fall, 3 - be high, 4 - be low */
         double timeout, /* time to wait [in seconds], if the input didn't change the value -1 is returned */
         short sub_thread)
{
    if (input_type == DIGITAL_INPUT)
    {
        if ((index < 0) || (index >= ZUCMOT_MAX_DIO))
            return -1;
    }
    else if (input_type == ANALOG_INPUT)
    {
        if ((index < 0) || (index >= ZUCMOT_MAX_AIO))
            return -1;
    }
    else if (input_type == TIO_DIGITAL_INPUT)
    {
        if ((index < 0) || (index >= 8))
            return -1;
    }
    else if (input_type == EXTIO_DIGITAL_INPUT)
    {
        if ((index < 0) || (index >= MAX_EXTIO_DI_NUM))
            return -1;
    }
    else if (input_type == MB_SLAVE_DIGITAL_INPUT)
    {
        if ((index < 0) || (index >= ZUCMOT_MAX_MODBUS_DIO))
            return -1;
    }
    else if (input_type == PN_DIGITAL_INPUT)
    {
        if ((index < 0) || (index >= ZUCMOT_MAX_PNDev_DIO))
            return -1;
    }
    else if (input_type == EIP_DIGITAL_INPUT)
    {
        if ((index < 0) || (index >= ZUCMOT_NEWMODE_MAX_EIP_DIO))
            return -1;
    }

    ZUC_AUX_INPUT_WAIT wait_msg;

    flush_segments();

    wait_msg.line_num = line_num;

    wait_msg.index = index;
    wait_msg.input_type = input_type;
    wait_msg.wait_type = wait_type;
    wait_msg.timeout = timeout;

    if (sub_thread)
    {
        threadInterpList[sub_thread - 1].nmlcmd_list->set_line_number(line_num);
        threadInterpList[sub_thread - 1].nmlcmd_list->append(wait_msg);
    }
    else
    {
        interp_list.set_line_number(line_num);
        interp_list.append(wait_msg);
    }
    return 0;
}

int UNLOCK_ROTARY(int line_number, int joint_num)
{
    ZUC_TRAJ_LINEAR_MOVE m;
    // first, set up a zero length move to interrupt blending and get to final position
    m.type = ZUC_MOTION_TYPE_TRAVERSE;
    m.feed_mode = 0;
    m.end = to_ext_pose(canon.endPoint.x,
                        canon.endPoint.y,
                        canon.endPoint.z,
                        canon.endPoint.a,
                        canon.endPoint.b,
                        canon.endPoint.c,
                        canon.endPoint.u,
                        canon.endPoint.v,
                        canon.endPoint.w);
    m.vel = m.acc = 1;  // nonzero but otherwise doesn't matter
    m.indexrotary = -1;

    // issue it
    int old_feed_mode = canon.feed_mode;
    if (canon.feed_mode)
        STOP_SPEED_FEED_SYNCH();
    interp_list.set_line_number(line_number);
    interp_list.append(m);
    // no need to update endpoint
    if (old_feed_mode)
        START_SPEED_FEED_SYNCH(canon.linearFeedRate, 1);

    // now, the next move is the real indexing move, so be ready
    canon.rotary_unlock_for_traverse = joint_num;
    return 0;
}

int LOCK_ROTARY(int line_number, int joint_num)
{
    canon.rotary_unlock_for_traverse = -1;
    return 0;
}

/* PLUGIN_CALL queues a Python tuple for execution by task
 * the tuple is expected to be already pickled
 * The tuple format is: (callable,tupleargs,keywordargs)
 */
void PLUGIN_CALL(int len, const char* call) {}

void IO_PLUGIN_CALL(int len, const char* call) {}
